Classes
class	sp::KF

class	sp::EKF

class	sp::UKF

Functions
	sp::KF::KF (arma::uword _N, arma::uword _M, arma::uword _L)

	sp::KF::~KF ()

void	sp::KF::clear (void)

void	sp::KF::set_state_vec (const arma::mat &_x)

void	sp::KF::set_trans_mat (const arma::mat &_A)

void	sp::KF::set_control_mat (const arma::mat &_B)

void	sp::KF::set_meas_mat (const arma::mat &_H)

void	sp::KF::set_err_cov (const arma::mat &_P)

void	sp::KF::set_proc_noise (const arma::mat &_Q)

void	sp::KF::set_meas_noise (const arma::mat &_R)

void	sp::KF::set_kalman_gain (const arma::mat &_K)

void	sp::KF::set_trans_fcn (fcn_v _f)

void	sp::KF::set_meas_fcn (fcn_v _h)

arma::mat	sp::KF::get_state_vec (void)

arma::mat	sp::KF::get_err (void)

arma::mat	sp::KF::get_kalman_gain (void)

arma::mat	sp::KF::get_err_cov (void)

void	sp::KF::predict (const arma::mat u)

void	sp::KF::predict (void)

void	sp::KF::update (const arma::mat z)

void	sp::KF::rts_smooth (const arma::mat &Xf, const arma::cube &Pf, arma::mat &Xs, arma::cube &Ps)

	sp::EKF::EKF (arma::uword _N, arma::uword _M, arma::uword _L)

void	sp::EKF::set_diff_step (double _dx)

void	sp::EKF::set_state_jac (fcn_m _f)

void	sp::EKF::set_meas_jac (fcn_m _h)

void	sp::EKF::jacobian_diff (arma::mat &_F, fcn_v _f, const arma::mat &_x)

void	sp::EKF::jacobian_diff (arma::mat &_F, fcn_v _f)

void	sp::EKF::jacobian_analytical (arma::mat &_F, fcn_m _f_m, const arma::mat &_x)

void	sp::EKF::jacobian_analytical (arma::mat &_F, fcn_m _f_m)

void	sp::EKF::predict (const arma::mat u)

void	sp::EKF::predict (void)

void	sp::EKF::update (const arma::mat z)

void	sp::EKF::rts_smooth (const arma::mat &Xf, const arma::cube &Pf, arma::mat &Xs, arma::cube &Ps)

	sp::UKF::UKF (arma::uword _N, arma::uword _M, arma::uword _L)

void	sp::UKF::set_alpha (double _a)

void	sp::UKF::set_beta (double _b)

void	sp::UKF::set_kappa (double _k)

void	sp::UKF::set_lambda (double _l)

void	sp::UKF::update_weights (void)

void	sp::UKF::update_sigma (const arma::mat &_x, const arma::mat &_P)

arma::mat	sp::UKF::ut (const arma::mat &_x, const arma::mat &_P, const fcn_v _f)

void	sp::UKF::predict (const arma::mat u)

void	sp::UKF::predict (void)

void	sp::UKF::update (const arma::mat z)

void	sp::UKF::rts_smooth (const arma::mat &Xf, const arma::cube &Pf, arma::mat &Xs, arma::cube &Ps)

Variables
arma::uword	sp::KF::N

arma::uword	sp::KF::M

arma::uword	sp::KF::L

bool	sp::KF::lin_proc

bool	sp::KF::lin_meas

arma::mat	sp::KF::x

arma::mat	sp::KF::z_err

arma::mat	sp::KF::A

arma::mat	sp::KF::B

arma::mat	sp::KF::H

arma::mat	sp::KF::P

arma::mat	sp::KF::Q

arma::mat	sp::KF::R

arma::mat	sp::KF::K

fcn_v	sp::KF::f

fcn_v	sp::KF::h

fcn_m	sp::EKF::f_jac

fcn_m	sp::EKF::h_jac

double	sp::EKF::dx

double	sp::UKF::alpha

double	sp::UKF::beta

double	sp::UKF::kappa

double	sp::UKF::lambda

arma::mat	sp::UKF::X

arma::mat	sp::UKF::S

arma::mat	sp::UKF::C

arma::vec	sp::UKF::Wx

arma::vec	sp::UKF::Wp

Detailed Description

Function Documentation

◆ clear()

void sp::KF::clear ( void )

inline

Clear the internal states and pointer.

Definition at line 187 of file kalman.h.

◆ EKF()

sp::EKF::EKF	(	arma::uword	_N,
		arma::uword	_M,
		arma::uword	_L
	)

inline

Definition at line 339 of file kalman.h.

◆ get_err()

arma::mat sp::KF::get_err ( void )

inline

Examples:: kalman_linear.cpp, and kalman_UKF.cpp.

Definition at line 238 of file kalman.h.

References sp::KF::z_err.

Referenced by main().

◆ get_err_cov()

arma::mat sp::KF::get_err_cov ( void )

inline

Examples:: kalman_linear.cpp, and kalman_UKF.cpp.

Definition at line 240 of file kalman.h.

References sp::KF::P.

Referenced by main().

◆ get_kalman_gain()

arma::mat sp::KF::get_kalman_gain ( void )

inline

Examples:: kalman_UKF.cpp.

Definition at line 239 of file kalman.h.

References sp::KF::K.

Referenced by main().

◆ get_state_vec()

arma::mat sp::KF::get_state_vec ( void )

inline

Examples:: kalman_linear.cpp, and kalman_UKF.cpp.

Definition at line 237 of file kalman.h.

References sp::KF::x.

Referenced by main().

◆ jacobian_analytical() [1/2]

void sp::EKF::jacobian_analytical	(	arma::mat &	_F,
		fcn_m	_f_m,
		const arma::mat &	_x
	)

inline

Evaluate Jacobian matrix using analytical jacobian.

Parameters

_F	Jacobian matrix d/dx evaluated at x
_f_m	Jacobian function matrix
_x	State vector

Definition at line 406 of file kalman.h.

References err_handler.

◆ jacobian_analytical() [2/2]

void sp::EKF::jacobian_analytical	(	arma::mat &	_F,
		fcn_m	_f_m
	)

inline

Evaluate Jacobian matrix using analytical jacobian.

Parameters

_F	Jacobian matrix d/dx evaluated at x
_f_m	Jacobian function matrix

Definition at line 427 of file kalman.h.

References sp::KF::x.

◆ jacobian_diff() [1/2]

void sp::EKF::jacobian_diff	(	arma::mat &	_F,
		fcn_v	_f,
		const arma::mat &	_x
	)

inline

Calculate and evaluate Jacobian matrix using finite difference approximation.

Parameters

_F	Jacobian matrix d/dx evaluated at x
_f	Function vector [ f0(x,u,w) ... fN(x,u,w)]
_x	State vector

Alternative: Complex Step Diff: http://blogs.mathworks.com/cleve/2013/10/14/complex-step-differentiation/

Definition at line 367 of file kalman.h.

References err_handler.

◆ jacobian_diff() [2/2]

void sp::EKF::jacobian_diff	(	arma::mat &	_F,
		fcn_v	_f
	)

inline

Calculate and evaluate Jacobian matrix using finite difference approximation.

Parameters

_F	Jacobian matrix d/dx evaluated at x
_f	Function vector [ f0(x,u,w) ... fN(x,u,w)]

Definition at line 395 of file kalman.h.

References sp::KF::x.

◆ KF()

sp::KF::KF	(	arma::uword	_N,
		arma::uword	_M,
		arma::uword	_L
	)

inline

Constructor.

Definition at line 152 of file kalman.h.

◆ predict() [1/6]

void sp::KF::predict ( const arma::mat u )

inline

Predict the internal states using a control input.

Parameters

u	Input/control signal

Examples:: kalman_linear.cpp.

Definition at line 246 of file kalman.h.

References sp::KF::Q.

Referenced by main().

◆ predict() [2/6]

void sp::KF::predict ( void )

inline

Predict the internal states, no control.

Definition at line 255 of file kalman.h.

Referenced by sp::EKF::predict(), and sp::UKF::predict().

◆ predict() [3/6]

void sp::EKF::predict ( const arma::mat u )

inline

Predict the internal states using a control input.

Parameters

u	Input/control signal

Definition at line 436 of file kalman.h.

References sp::KF::A, sp::KF::B, sp::eval_fcn(), sp::KF::f, sp::KF::lin_proc, sp::KF::P, sp::KF::Q, and sp::KF::x.

Here is the call graph for this function:

◆ predict() [4/6]

void sp::EKF::predict ( void )

inline

Predict the internal states, no control.

Definition at line 459 of file kalman.h.

References sp::KF::L, and sp::KF::predict().

Here is the call graph for this function:

◆ predict() [5/6]

void sp::UKF::predict ( const arma::mat u )

inline

Predict the internal states using a control input.

Parameters

u	Input/control signal

Examples:: kalman_UKF.cpp.

Definition at line 669 of file kalman.h.

References sp::KF::A, sp::KF::B, sp::KF::f, sp::KF::lin_proc, sp::KF::P, sp::KF::Q, and sp::KF::x.

Referenced by main().

◆ predict() [6/6]

void sp::UKF::predict ( void )

inline

Predict the internal states, no control. Convenient function.

Definition at line 692 of file kalman.h.

References sp::KF::L, and sp::KF::predict().

Here is the call graph for this function:

◆ rts_smooth() [1/3]

void sp::KF::rts_smooth	(	const arma::mat &	Xf,
		const arma::cube &	Pf,
		arma::mat &	Xs,
		arma::cube &	Ps
	)

inline

Rauch-Tung-Striebel smoother. See http://www.lce.hut.fi/~ssarkka/course_k2011/pdf/course_booklet_2011.pdf.

Examples:: kalman_linear.cpp.

Definition at line 283 of file kalman.h.

References sp::KF::Q.

Referenced by main().

◆ rts_smooth() [2/3]

void sp::EKF::rts_smooth	(	const arma::mat &	Xf,
		const arma::cube &	Pf,
		arma::mat &	Xs,
		arma::cube &	Ps
	)

inline

Rauch-Tung-Striebel smoother. See http://www.lce.hut.fi/~ssarkka/course_k2011/pdf/course_booklet_2011.pdf.

Definition at line 505 of file kalman.h.

References sp::KF::A, sp::eval_fcn(), sp::KF::f, sp::KF::lin_proc, sp::KF::N, and sp::KF::Q.

Here is the call graph for this function:

◆ rts_smooth() [3/3]

void sp::UKF::rts_smooth	(	const arma::mat &	Xf,
		const arma::cube &	Pf,
		arma::mat &	Xs,
		arma::cube &	Ps
	)

inline

Rauch-Tung-Striebel smoother. See http://www.lce.hut.fi/~ssarkka/course_k2011/pdf/course_booklet_2011.pdf.

Examples:: kalman_UKF.cpp.

Definition at line 744 of file kalman.h.

References sp::KF::A, sp::KF::f, sp::KF::lin_proc, sp::KF::N, and sp::KF::Q.

Referenced by main().

◆ set_alpha()

void sp::UKF::set_alpha ( double _a )

inline

Definition at line 594 of file kalman.h.

◆ set_beta()

void sp::UKF::set_beta ( double _b )

inline

Definition at line 595 of file kalman.h.

◆ set_control_mat()

void sp::KF::set_control_mat ( const arma::mat & _B )

inline

Definition at line 202 of file kalman.h.

◆ set_diff_step()

void sp::EKF::set_diff_step ( double _dx )

inline

Definition at line 347 of file kalman.h.

◆ set_err_cov()

void sp::KF::set_err_cov ( const arma::mat & _P )

inline

Examples:: kalman_linear.cpp, and kalman_UKF.cpp.

Definition at line 210 of file kalman.h.

Referenced by main().

◆ set_kalman_gain()

void sp::KF::set_kalman_gain ( const arma::mat & _K )

inline

Definition at line 222 of file kalman.h.

◆ set_kappa()

void sp::UKF::set_kappa ( double _k )

inline

Definition at line 596 of file kalman.h.

◆ set_lambda()

void sp::UKF::set_lambda ( double _l )

inline

Definition at line 597 of file kalman.h.

◆ set_meas_fcn()

void sp::KF::set_meas_fcn ( fcn_v _h )

inline

Examples:: kalman_UKF.cpp.

Definition at line 231 of file kalman.h.

Referenced by main().

◆ set_meas_jac()

void sp::EKF::set_meas_jac ( fcn_m _h )

inline

Definition at line 352 of file kalman.h.

◆ set_meas_mat()

void sp::KF::set_meas_mat ( const arma::mat & _H )

inline

Examples:: kalman_linear.cpp.

Definition at line 206 of file kalman.h.

Referenced by main().

◆ set_meas_noise()

void sp::KF::set_meas_noise ( const arma::mat & _R )

inline

Examples:: kalman_linear.cpp, and kalman_UKF.cpp.

Definition at line 218 of file kalman.h.

Referenced by main().

◆ set_proc_noise()

void sp::KF::set_proc_noise ( const arma::mat & _Q )

inline

Examples:: kalman_linear.cpp, and kalman_UKF.cpp.

Definition at line 214 of file kalman.h.

Referenced by main().

◆ set_state_jac()

void sp::EKF::set_state_jac ( fcn_m _f )

inline

Definition at line 348 of file kalman.h.

◆ set_state_vec()

void sp::KF::set_state_vec ( const arma::mat & _x )

inline

Examples:: kalman_linear.cpp, and kalman_UKF.cpp.

Definition at line 197 of file kalman.h.

Referenced by main().

◆ set_trans_fcn()

void sp::KF::set_trans_fcn ( fcn_v _f )

inline

Definition at line 226 of file kalman.h.

◆ set_trans_mat()

void sp::KF::set_trans_mat ( const arma::mat & _A )

inline

Examples:: kalman_linear.cpp, and kalman_UKF.cpp.

Definition at line 198 of file kalman.h.

Referenced by main().

◆ UKF()

sp::UKF::UKF	(	arma::uword	_N,
		arma::uword	_M,
		arma::uword	_L
	)

inline

Definition at line 577 of file kalman.h.

◆ update() [1/3]

void sp::KF::update ( const arma::mat z )

inline

Correct and update the internal states.

Examples:: kalman_linear.cpp.

Definition at line 264 of file kalman.h.

References sp::KF::N, sp::KF::R, sp::KF::x, and sp::KF::z_err.

Referenced by main().

◆ update() [2/3]

void sp::EKF::update ( const arma::mat z )

inline

Correct and update the internal states. EKF.

Definition at line 468 of file kalman.h.

References sp::eval_fcn(), sp::KF::H, sp::KF::h, sp::KF::K, sp::KF::lin_meas, sp::KF::M, sp::KF::N, sp::KF::P, sp::KF::R, sp::KF::x, and sp::KF::z_err.

Here is the call graph for this function:

◆ update() [3/3]

void sp::UKF::update ( const arma::mat z )

inline

Correct and update the internal states. UKF.

Examples:: kalman_UKF.cpp.

Definition at line 701 of file kalman.h.

References sp::KF::H, sp::KF::h, sp::KF::K, sp::KF::lin_meas, sp::KF::M, sp::KF::N, sp::KF::P, sp::KF::R, sp::KF::x, and sp::KF::z_err.

Referenced by main().

◆ update_sigma()

void sp::UKF::update_sigma	(	const arma::mat &	_x,
		const arma::mat &	_P
	)

inline

Calculate sigma points around a reference point.

Parameters

_x	State matrix
_P	Covariance matrix

Definition at line 623 of file kalman.h.

References sp::KF::N.

◆ update_weights()

void sp::UKF::update_weights ( void )

inline

Calculate sigma point weights.

Definition at line 602 of file kalman.h.

References sp::KF::N.

◆ ut()

arma::mat sp::UKF::ut	(	const arma::mat &	_x,
		const arma::mat &	_P,
		const fcn_v	_f
	)

inline

Calculate unscented transform.

Definition at line 636 of file kalman.h.

References sp::eval_fcn(), and sp::KF::N.

Here is the call graph for this function:

◆ ~KF()

sp::KF::~KF ( )

inline

Destructor.

Definition at line 182 of file kalman.h.

Variable Documentation

◆ A

arma::mat sp::KF::A

protected

State transition matrix.

Definition at line 139 of file kalman.h.

Referenced by sp::EKF::predict(), sp::UKF::predict(), sp::EKF::rts_smooth(), and sp::UKF::rts_smooth().

◆ alpha

double sp::UKF::alpha

protected

Spread factor of sigma points.

Definition at line 566 of file kalman.h.

◆ B

arma::mat sp::KF::B

protected

Input matrix.

Definition at line 140 of file kalman.h.

Referenced by sp::EKF::predict(), and sp::UKF::predict().

◆ beta

double sp::UKF::beta

protected

x distr. prior knowledge factor

Definition at line 567 of file kalman.h.

◆ C

arma::mat sp::UKF::C

protected

Cross covariance input-output.

Definition at line 573 of file kalman.h.

◆ dx

double sp::EKF::dx

protected

Finite difference approximation step size.

Definition at line 337 of file kalman.h.

◆ f

fcn_v sp::KF::f

protected

Vector of Kalman state transition functions.

Definition at line 146 of file kalman.h.

Referenced by sp::EKF::predict(), sp::UKF::predict(), sp::EKF::rts_smooth(), and sp::UKF::rts_smooth().

◆ f_jac

fcn_m sp::EKF::f_jac

protected

Matrix of Extended Kalman state transition jacobian.

Definition at line 335 of file kalman.h.

◆ H

arma::mat sp::KF::H

protected

Measurement matrix.

Definition at line 141 of file kalman.h.

Referenced by sp::EKF::update(), and sp::UKF::update().

◆ h

fcn_v sp::KF::h

protected

Vector of Kalman measurement functions.

Definition at line 147 of file kalman.h.

Referenced by sp::EKF::update(), and sp::UKF::update().

◆ h_jac

fcn_m sp::EKF::h_jac

protected

Matrix of Extended Kalman measurement transition jacobian.

Definition at line 336 of file kalman.h.

◆ K

arma::mat sp::KF::K

protected

Kalman gain vector.

Definition at line 145 of file kalman.h.

Referenced by sp::KF::get_kalman_gain(), sp::EKF::update(), and sp::UKF::update().

◆ kappa

double sp::UKF::kappa

protected

Scaling par.

Definition at line 568 of file kalman.h.

◆ L

arma::uword sp::KF::L

protected

Number of measurements/observations.

Definition at line 134 of file kalman.h.

Referenced by sp::EKF::predict(), and sp::UKF::predict().

◆ lambda

double sp::UKF::lambda

protected

Definition at line 569 of file kalman.h.

◆ lin_meas

bool sp::KF::lin_meas

protected

Linearity flag for measurement.

Definition at line 136 of file kalman.h.

Referenced by sp::EKF::update(), and sp::UKF::update().

◆ lin_proc

bool sp::KF::lin_proc

protected

Linearity flag for process.

Definition at line 135 of file kalman.h.

Referenced by sp::EKF::predict(), sp::UKF::predict(), sp::EKF::rts_smooth(), and sp::UKF::rts_smooth().

◆ M

arma::uword sp::KF::M

protected

Number of inputs.

Definition at line 133 of file kalman.h.

Referenced by sp::EKF::update(), and sp::UKF::update().

◆ N

arma::uword sp::KF::N

protected

Number of states.

Definition at line 132 of file kalman.h.

Referenced by sp::EKF::rts_smooth(), sp::UKF::rts_smooth(), sp::KF::update(), sp::EKF::update(), sp::UKF::update(), sp::UKF::update_sigma(), sp::UKF::update_weights(), and sp::UKF::ut().

◆ P

arma::mat sp::KF::P

protected

Error covariance matrix (estimated accuracy)

Definition at line 142 of file kalman.h.

Referenced by sp::KF::get_err_cov(), sp::EKF::predict(), sp::UKF::predict(), sp::EKF::update(), and sp::UKF::update().

◆ Q

arma::mat sp::KF::Q

protected

Process noise.

Definition at line 143 of file kalman.h.

Referenced by sp::KF::predict(), sp::EKF::predict(), sp::UKF::predict(), sp::KF::rts_smooth(), sp::EKF::rts_smooth(), and sp::UKF::rts_smooth().

◆ R

arma::mat sp::KF::R

protected

Measurement noise.

Definition at line 144 of file kalman.h.

Referenced by sp::KF::update(), sp::EKF::update(), and sp::UKF::update().

◆ S

arma::mat sp::UKF::S

protected

Output covariance.

Definition at line 572 of file kalman.h.

◆ Wp

arma::vec sp::UKF::Wp

protected

Weights covariance.

Definition at line 575 of file kalman.h.

◆ Wx

arma::vec sp::UKF::Wx

protected

Weights states.

Definition at line 574 of file kalman.h.

◆ x

arma::mat sp::KF::x

protected

State vector.

Definition at line 137 of file kalman.h.

Referenced by sp::KF::get_state_vec(), sp::EKF::jacobian_analytical(), sp::EKF::jacobian_diff(), sp::EKF::predict(), sp::UKF::predict(), sp::KF::update(), sp::EKF::update(), and sp::UKF::update().

◆ X

arma::mat sp::UKF::X

protected

Sigma points.

Definition at line 571 of file kalman.h.

◆ z_err

arma::mat sp::KF::z_err

protected

Prediction error.

Definition at line 138 of file kalman.h.

Referenced by sp::KF::get_err(), sp::KF::update(), sp::EKF::update(), and sp::UKF::update().

Classes

Functions

Variables

Detailed Description

Function Documentation

◆ clear()

◆ EKF()

◆ get_err()

◆ get_err_cov()

◆ get_kalman_gain()

◆ get_state_vec()

◆ jacobian_analytical() [1/2]

◆ jacobian_analytical() [2/2]

◆ jacobian_diff() [1/2]

◆ jacobian_diff() [2/2]

◆ KF()

◆ predict() [1/6]

◆ predict() [2/6]

◆ predict() [3/6]

◆ predict() [4/6]

◆ predict() [5/6]

◆ predict() [6/6]

◆ rts_smooth() [1/3]

◆ rts_smooth() [2/3]

◆ rts_smooth() [3/3]

◆ set_alpha()

◆ set_beta()

◆ set_control_mat()

◆ set_diff_step()

◆ set_err_cov()

◆ set_kalman_gain()

◆ set_kappa()

◆ set_lambda()

◆ set_meas_fcn()

◆ set_meas_jac()

◆ set_meas_mat()

◆ set_meas_noise()

◆ set_proc_noise()

◆ set_state_jac()

◆ set_state_vec()

◆ set_trans_fcn()

◆ set_trans_mat()

◆ UKF()

◆ update() [1/3]

◆ update() [2/3]

◆ update() [3/3]

◆ update_sigma()

◆ update_weights()

◆ ut()

◆ ~KF()

Variable Documentation

◆ A

◆ alpha

◆ B

◆ beta

◆ C

◆ dx

◆ f

◆ f_jac

◆ H

◆ h

◆ h_jac

◆ K

◆ kappa

◆ L

◆ lambda

◆ lin_meas

◆ lin_proc

◆ M

◆ N

◆ P

◆ Q

◆ R

◆ S

◆ Wp

◆ Wx

◆ x

◆ X

◆ z_err