gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz > Class Template Reference

#include <lqsensorcost.h>

Inheritance diagram for gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >:

Collaboration diagram for gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >:

List of all members.

Public Types
typedef Matrix< double, _ng, 1 >	Vectorgd
typedef Matrix< double, _ng, _nx >	Matrixgxd
typedef Matrix< double, _ng, _nu >	Matrixgud
typedef Matrix< double, _ng, _np >	Matrixgpd
typedef Matrix< double, _ng, _nz >	Matrixgzd
typedef Matrix< double, _nx, 1 >	Vectornd
typedef Matrix< double, _nu, 1 >	Vectorcd
typedef Matrix< double, _np, 1 >	Vectormd
typedef Matrix< double, _nx, _nx >	Matrixnd
typedef Matrix< double, _nx, _nu >	Matrixncd
typedef Matrix< double, _nu, _nx >	Matrixcnd
typedef Matrix< double, _nu, _nu >	Matrixcd
typedef Matrix< double, _np, _np >	Matrixmd
typedef Matrix< double, _nx, _np >	Matrixnmd
typedef Matrix< double, _np, _nx >	Matrixmnd
typedef Matrix< double, _nz, 1 >	Vectorrd
typedef Matrix< double, _nz, _nz >	Matrixrd
typedef Matrix< double, _nz, _nx >	Matrixrnd
typedef Matrix< double, _nz, _nu >	Matrixrcd
typedef Matrix< double, _nz, _np >	Matrixrmd
Public Member Functions
	LqSensorCost (System< T, _nx, _nu, _np > &sys, Manifold< Tz, _nz > &Z, bool diag=true)
void	UpdateGains ()
double	L (double t, const Tz &z, const Vectornd &w, const Vectormd &p, double h, int sensor_index, Vectornd *Lw=0, Matrixnd *Lww=0, Vectormd *Lp=0, Matrixmd *Lpp=0, Vectorrd *Lz=0, Matrixrd *Lzz=0, Matrixmnd *Lpw=0, Matrixrnd *Lzw=0, Matrixrmd *Lzp=0)
double	Lp (const Vectormd &p, Vectormd *Lp=0, Matrixmd *Lpp=0)
bool	Res (Vectorgd &g, double t, const Tz &z, const Vectornd &w, const Vectormd &p, double h, int sensor_index, Matrixgxd *dgdw=0, Matrixgpd *dgdp=0, Matrixgzd *dgdz=0)
bool	Resp (Vectormd &gp, const Vectormd &p, Matrixmd *dgdp=0)
void	SetReference (const vector< Tz > *zs, Vectormd *mup=0)
Public Attributes
bool	diag
	are the Q, Qf, and R matrices diagonal (true by default)
Matrixrd	R
	inverse of measurement covariance matrix
Matrixnd	S
	inverse of noise covariance matrix
Matrixmd	P
	inverse of parameter covariance matrix (set to zero if nothing is known about parameters before hand)
Matrixnd	Ssqrt
	S sqrt.
Matrixrd	Rsqrt
	R sqrt.
Matrixmd	Psqrt
	P sqrt.
const vector< Tz > *	zs
	measured sensor data
Vectormd *	mup
	Prior for parameter data.
Protected Attributes
Vectorrd	dz
	measurement error (as a tangent vector)
Vectormd	dp
	parameter error

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>
class gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >

---

Member Typedef Documentation

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

typedef Matrix<double, _nu, _nu> gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Matrixcd

Reimplemented from gcop::LsSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >.

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

typedef Matrix<double, _nu, _nx> gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Matrixcnd

Reimplemented from gcop::LsSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >.

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

typedef Matrix<double, _ng, _np> gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Matrixgpd

Reimplemented from gcop::LsSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >.

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

typedef Matrix<double, _ng, _nu> gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Matrixgud

Reimplemented from gcop::LsSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >.

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

typedef Matrix<double, _ng, _nx> gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Matrixgxd

Reimplemented from gcop::LsSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >.

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

typedef Matrix<double, _ng, _nz> gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Matrixgzd

Reimplemented from gcop::LsSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >.

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

typedef Matrix<double, _np, _np> gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Matrixmd

Reimplemented from gcop::LsSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >.

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

typedef Matrix<double, _np, _nx> gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Matrixmnd

Reimplemented from gcop::LsSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >.

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

typedef Matrix<double, _nx, _nu> gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Matrixncd

Reimplemented from gcop::LsSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >.

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

typedef Matrix<double, _nx, _nx> gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Matrixnd

Reimplemented from gcop::LsSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >.

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

typedef Matrix<double, _nx, _np> gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Matrixnmd

Reimplemented from gcop::LsSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >.

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

typedef Matrix<double, _nz, _nu> gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Matrixrcd

Reimplemented from gcop::LsSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >.

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

typedef Matrix<double, _nz, _nz> gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Matrixrd

Reimplemented from gcop::LsSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >.

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

typedef Matrix<double, _nz, _np> gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Matrixrmd

Reimplemented from gcop::LsSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >.

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

typedef Matrix<double, _nz, _nx> gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Matrixrnd

Reimplemented from gcop::LsSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >.

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

typedef Matrix<double, _nu, 1> gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Vectorcd

Reimplemented from gcop::LsSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >.

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

typedef Matrix<double, _ng, 1> gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Vectorgd

Reimplemented from gcop::LsSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >.

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

typedef Matrix<double, _np, 1> gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Vectormd

Reimplemented from gcop::LsSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >.

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

typedef Matrix<double, _nx, 1> gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Vectornd

Reimplemented from gcop::LsSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >.

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

typedef Matrix<double, _nz, 1> gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Vectorrd

Reimplemented from gcop::LsSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >.

---

Constructor & Destructor Documentation

template<typename T, int _nx, int _nu, int _np, int _ng, typename Tz, int _nz>

gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::LqSensorCost	(	System< T, _nx, _nu, _np > &	sys,
		Manifold< Tz, _nz > &	Z,
		bool	diag = true
	)

Linear-quadratic cost on a sensor manifold. Use this constructor for dynamic-size control problem, i.e. LqCost<T>(X, U, tf, xf, ...)

Parameters:

sys	provides system manifold
sensor	provides sensor manifold
tf	final time
diag	whether the cost matrices are diagonal?

References gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::dp, gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::mup, gcop::Manifold< T, _n >::n, gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::P, gcop::System< T, _nx, _nu, _np >::P, gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Psqrt, gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::R, gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Rsqrt, gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::S, gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Ssqrt, gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::UpdateGains(), gcop::System< T, _nx, _nu, _np >::X, and gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::zs.

---

Member Function Documentation

template<typename T , int _nx, int _nu, int _np, int _ng, typename Tz, int _nz>

double gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::L	(	double	t,
		const Tz &	z,
		const Vectornd &	w,
		const Vectormd &	p,
		double	h,
		int	sensor_index,
		Vectornd *	Lw = 0,
		Matrixnd *	Lww = 0,
		Vectormd *	Lp = 0,
		Matrixmd *	Lpp = 0,
		Vectorrd *	Lz = 0,
		Matrixrd *	Lzz = 0,
		Matrixmnd *	Lpw = 0,
		Matrixrnd *	Lzw = 0,
		Matrixrmd *	Lzp = 0
	)

template<typename T , int _nx, int _nu, int _np, int _ng, typename Tz , int _nz>

double gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Lp	(	const Vectormd &	p,
		Vectormd *	Lp = 0,
		Matrixmd *	Lpp = 0
	)		[virtual]

Part of cost function which is independent of time and only dependent on parameters The net cost for a trajectory is sum i:1->N [L_i] + Lp

Parameters:

p	parameters
Lp	derivative wrt p
Lpp	derivative wrt p,p

Reimplemented from gcop::SensorCost< T, _nx, _nu, _np, Tz, _nz >.

template<typename T , int _nx, int _nu, int _np, int _ng, typename Tz, int _nz>

bool gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Res	(	Vectorgd &	g,
		double	t,
		const Tz &	z,
		const Vectornd &	w,
		const Vectormd &	p,
		double	h,
		int	sensor_index,
		Matrixgxd *	dgdw = 0,
		Matrixgpd *	dgdp = 0,
		Matrixgzd *	dgdz = 0
	)

References np.

template<typename T , int _nx, int _nu, int _np, int _ng, typename Tz , int _nz>

bool gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Resp	(	Vectormd &	gp,
		const Vectormd &	p,
		Matrixmd *	dgdp = 0
	)		[virtual]

Residual Cost independent of time and only dependent on parameters

Parameters:

g	residual for parameters
p	parameter
dgdp	derivative wrt p

Reimplemented from gcop::LsSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >.

template<typename T , int _nx, int _nu, int _np, int _ng, typename Tz, int _nz>

void gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::SetReference	(	const vector< Tz > *	zs,
		Vectormd *	mup = 0
	)

Set observed measurements for the cost

Parameters:

zs	observed measurements
mup	mean parameters (Prior for parameters)

template<typename T , int _nx, int _nu, int _np, int _ng, typename Tz , int _nz>

void gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::UpdateGains

(

)

Updates the square root gains, i.e. sets Qsqrt = Q.sqrt(), etc...

Referenced by gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::LqSensorCost().

---

Member Data Documentation

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

bool gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::diag

are the Q, Qf, and R matrices diagonal (true by default)

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

Vectormd gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::dp [protected]

parameter error

Referenced by gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::LqSensorCost().

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

Vectorrd gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::dz [protected]

measurement error (as a tangent vector)

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

Vectormd* gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::mup

Prior for parameter data.

Referenced by gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::LqSensorCost().

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

Matrixmd gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::P

inverse of parameter covariance matrix (set to zero if nothing is known about parameters before hand)

Referenced by gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::LqSensorCost().

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

Matrixmd gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Psqrt

P sqrt.

Referenced by gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::LqSensorCost().

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

Matrixrd gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::R

inverse of measurement covariance matrix

Referenced by gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::LqSensorCost().

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

Matrixrd gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Rsqrt

R sqrt.

Referenced by gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::LqSensorCost().

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

Matrixnd gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::S

inverse of noise covariance matrix

Referenced by gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::LqSensorCost().

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

Matrixnd gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::Ssqrt

S sqrt.

Referenced by gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::LqSensorCost().

template<typename T = VectorXd, int _nx = Dynamic, int _nu = Dynamic, int _np = Dynamic, int _ng = Dynamic, typename Tz = VectorXd, int _nz = Dynamic>

const vector<Tz>* gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::zs

measured sensor data

Referenced by gcop::LqSensorCost< T, _nx, _nu, _np, _ng, Tz, _nz >::LqSensorCost().

---

The documentation for this class was generated from the following file:

• lib/systems/costs/lqsensorcost.h