DUNE::Navigation::KalmanFilter Class Reference

Public Member Functions
	KalmanFilter (void)
	KalmanFilter (Math::Matrix &A, Math::Matrix &C, Math::Matrix &P, Math::Matrix &Q)
void	initialize (Math::Matrix &P0, Math::Matrix &x0)
void	reset (short num_states, short num_outputs)
bool	resize (short num_outputs)
void	normalize (void)
void	predict (Math::Matrix &b, Math::Matrix &u)
void	predict (void)
int	update (float threshold)
double	getState (short pos) const
Math::Matrix	getState (void) const
void	setState (short pos, double value)
void	resetState (void)
Math::Matrix	getStateTransition (void) const
void	setStateTransition (Math::Matrix a)
Math::Matrix	getCovarianceTransition (void) const
void	setCovarianceTransition (Math::Matrix a)
void	setTransitions (Math::Matrix a)
void	resetOutputs (void)
double	getOutput (short pos) const
void	setOutput (short pos, double value)
double	getInnovation (short pos) const
void	setInnovation (short pos, double value)
Math::Matrix	getObservation (size_t i1, size_t i2, size_t j1, size_t j2) const
Math::Matrix	getObservation (void) const
void	setObservation (short ln, short cl, double value)
double	getCovariance (short ln, short cl) const
double	getCovariance (short in) const
Math::Matrix	getCovariance (size_t i1, size_t i2, size_t j1, size_t j2) const
Math::Matrix	getCovariance (void) const
void	setCovariance (short ln, short cl, double value)
void	setCovariance (short in, double value)
void	setCovariance (double value)
void	resetCovariance (short in)
void	setProcessNoise (short ln, short cl, double value)
void	setProcessNoise (short in, double value)
void	setProcessNoise (double value)
void	setMeasurementNoise (short ln, short cl, double value)
void	setMeasurementNoise (short in, double value)
void	setMeasurementNoise (double value)

Constructor & Destructor Documentation

DUNE::Navigation::KalmanFilter::KalmanFilter

(

void

)

Constructor.

DUNE::Navigation::KalmanFilter::KalmanFilter	(	Math::Matrix &	A,
		Math::Matrix &	C,
		Math::Matrix &	P,
		Math::Matrix &	Q
	)

Constructor.

Parameters

A	state-transition model.
C	observation model.
P	state covariance matrix.
Q	process noise covariance matrix.

References DUNE::Math::Matrix::resizeAndFill(), and DUNE::Math::Matrix::rows().

Member Function Documentation

double DUNE::Navigation::KalmanFilter::getCovariance ( short  ln, short  cl  ) const

inline

Get covariance matrix value.

Parameters

ln	row index.
cl	column index.

Returns

covariance matrix value.

References DUNE::Math::Matrix::columns(), and DUNE::Math::Matrix::rows().

Referenced by DUNE::Navigation::BasicNavigation::checkUncertainty(), Navigation::General::ROV::Task::logData(), Navigation::AUV::Navigation::Task::logData(), DUNE::Navigation::BasicNavigation::onDispatchNavigation(), DUNE::Navigation::BasicNavigation::runKalmanLBL(), and Navigation::AUV::Navigation::Task::task().

double DUNE::Navigation::KalmanFilter::getCovariance ( short  in ) const

inline

Get covariance matrix value.

Parameters

in	row and column index.

Returns

covariance matrix value.

Math::Matrix DUNE::Navigation::KalmanFilter::getCovariance ( size_t  i1, size_t  i2, size_t  j1, size_t  j2  ) const

inline

Get state covariance matrix.

Parameters

i1	lower row index.
i2	upper row index.
j1	lower column index.
j2	upper column index.

Returns

state covariance submatrix.

References DUNE::Math::Matrix::get().

Math::Matrix DUNE::Navigation::KalmanFilter::getCovariance ( void  ) const

inline

Get state covariance matrix.

Returns

state covariance matrix.

Math::Matrix DUNE::Navigation::KalmanFilter::getCovarianceTransition ( void  ) const

inline

Get state covariance transition matrix.

Returns

state covariance transition matrix.

double DUNE::Navigation::KalmanFilter::getInnovation ( short  pos ) const

inline

Get innovation matrix value.

Parameters

pos	matrix index.

Returns

innovation matrix value.

Referenced by Navigation::General::ROV::Task::logData(), and Navigation::AUV::Navigation::Task::logData().

Math::Matrix DUNE::Navigation::KalmanFilter::getObservation ( size_t  i1, size_t  i2, size_t  j1, size_t  j2  ) const

inline

Get observation model submatrix.

Parameters

i1	lower row index.
i2	upper row index.
j1	lower column index.
j2	upper column index.

Returns

output transition submatrix.

References DUNE::Math::Matrix::get().

Math::Matrix DUNE::Navigation::KalmanFilter::getObservation ( void  ) const

inline

Get output transition matrix.

Returns

output transition matrix.

double DUNE::Navigation::KalmanFilter::getOutput ( short  pos ) const

inline

Get output matrix value.

Parameters

pos	matrix index.

Returns

output matrix value.

References DUNE::Math::Matrix::rows().

Referenced by Navigation::General::ROV::Task::task(), and Navigation::AUV::Navigation::Task::task().

double DUNE::Navigation::KalmanFilter::getState ( short  pos ) const

inline

Get filter state value.

Parameters

pos	matrix index.

Returns

state matrix value.

Referenced by DUNE::Navigation::BasicNavigation::consume(), Navigation::AUV::Navigation::Task::getBiasedHeading(), Navigation::General::ROV::Task::logData(), Navigation::AUV::Navigation::Task::logData(), DUNE::Navigation::BasicNavigation::onDispatchNavigation(), Navigation::AUV::Navigation::Task::setTransition(), Navigation::General::ROV::Task::task(), and Navigation::AUV::Navigation::Task::task().

Math::Matrix DUNE::Navigation::KalmanFilter::getState ( void  ) const

inline

Get state matrix.

Returns

state matrix.

Math::Matrix DUNE::Navigation::KalmanFilter::getStateTransition ( void  ) const

inline

Get state transition matrix.

Returns

state transition matrix.

void DUNE::Navigation::KalmanFilter::initialize	(	Math::Matrix &	P0,
		Math::Matrix &	x0
	)

Set initial conditions (state and covariance matrix).

Parameters

P0	covariance.
x0	state.

References DUNE::Math::Matrix::columns(), and DUNE::Math::Matrix::rows().

void DUNE::Navigation::KalmanFilter::normalize

(

void

)

Keep the state covariance matrix symmetric.

void DUNE::Navigation::KalmanFilter::predict	(	Math::Matrix &	b,
		Math::Matrix &	u
	)

Predict the state at the next timestep subject to control input.

Parameters

b	control input matrix.
u	input matrix.

References DUNE::Math::Matrix::columns(), and DUNE::Math::Matrix::rows().

Referenced by Navigation::General::ROV::Task::task(), and Navigation::AUV::Navigation::Task::task().

void DUNE::Navigation::KalmanFilter::predict

(

void

)

Predict the state at the next timestep assuming no input.

void DUNE::Navigation::KalmanFilter::reset	(	short	num_states,
		short	num_outputs
	)

Reset and resize all matrices.

Parameters

num_states	number of filter states.
num_outputs	number of filter output states.

References DUNE::Math::Matrix::identity(), DUNE::Math::Matrix::resizeAndFill(), and DUNE::Math::Matrix::rows().

Referenced by Navigation::General::ROV::Task::Task(), and Navigation::AUV::Navigation::Task::Task().

void DUNE::Navigation::KalmanFilter::resetCovariance

(

short

in

)

Reset covariance values.

Referenced by Navigation::AUV::Navigation::Task::consume(), and Navigation::AUV::Navigation::Task::task().

void DUNE::Navigation::KalmanFilter::resetOutputs

(

void

)

Reset output matrices.

References DUNE::Math::Matrix::fill().

Referenced by Navigation::General::ROV::Task::resetKalman(), and Navigation::AUV::Navigation::Task::resetKalman().

void DUNE::Navigation::KalmanFilter::resetState

(

void

)

Reset state matrix.

References DUNE::Math::Matrix::fill().

Referenced by DUNE::Navigation::BasicNavigation::setup().

bool DUNE::Navigation::KalmanFilter::resize

(

short

num_outputs

)

Resize number of state outputs.

Parameters

num_outputs

number of filter output states.

Returns

true if resized, false otherwise.

References DUNE::Math::Matrix::resizeAndKeep(), and DUNE::Math::Matrix::rows().

Referenced by Navigation::General::ROV::Task::onConsumeLblConfig(), and Navigation::AUV::Navigation::Task::onConsumeLblConfig().

void DUNE::Navigation::KalmanFilter::setCovariance	(	short	ln,
		short	cl,
		double	value
	)

Set state covariance matrix value.

Parameters

ln	row index.
cl	column index.
value	state covariance matrix value.

References DUNE::Math::Matrix::columns(), and DUNE::Math::Matrix::rows().

Referenced by Navigation::AUV::Navigation::Task::consume(), Navigation::General::ROV::Task::setup(), Navigation::AUV::Navigation::Task::setup(), and Navigation::AUV::Navigation::Task::task().

void DUNE::Navigation::KalmanFilter::setCovariance	(	short	in,
		double	value
	)

Set state covariance matrix value.

Parameters

in	row and column index.
value	state covariance matrix value.

void DUNE::Navigation::KalmanFilter::setCovariance

(

double

value

)

Set state covariance matrix value.

Parameters

value

state covariance matrix value.

void DUNE::Navigation::KalmanFilter::setCovarianceTransition

(

Math::Matrix

a

)

Set state covariance transition matrix.

Parameters

a	state covariance transition matrix.

References DUNE::Math::Matrix::columns(), and DUNE::Math::Matrix::rows().

Referenced by setTransitions(), Navigation::General::ROV::Task::task(), and Navigation::AUV::Navigation::Task::task().

void DUNE::Navigation::KalmanFilter::setInnovation	(	short	pos,
		double	value
	)

Set innovation matrix value.

Parameters

pos	matrix index.
value	innovation matrix value.

References DUNE::Math::Matrix::rows().

Referenced by DUNE::Navigation::BasicNavigation::runKalmanLBL(), Navigation::General::ROV::Task::task(), and Navigation::AUV::Navigation::Task::task().

void DUNE::Navigation::KalmanFilter::setMeasurementNoise	(	short	ln,
		short	cl,
		double	value
	)

Set measurement noise covariance matrix value.

Parameters

ln	row index.
cl	column index.
value	measurement noise covariance matrix value.

References DUNE::Math::Matrix::columns(), and DUNE::Math::Matrix::rows().

Referenced by Navigation::AUV::Navigation::Task::consume(), Navigation::General::ROV::Task::onUpdateParameters(), Navigation::AUV::Navigation::Task::onUpdateParameters(), and Navigation::AUV::Navigation::Task::updateKalmanParametersGps().

void DUNE::Navigation::KalmanFilter::setMeasurementNoise	(	short	in,
		double	value
	)

Set measurement noise covariance matrix value.

Parameters

in	row and column index.
value	measurement noise covariance matrix value.

References DUNE::Math::Matrix::rows().

void DUNE::Navigation::KalmanFilter::setMeasurementNoise

(

double

value

)

Set measurement noise covariance matrix value.

Parameters

value

measurement noise covariance matrix value.

References DUNE::Math::Matrix::rows().

void DUNE::Navigation::KalmanFilter::setObservation	(	short	ln,
		short	cl,
		double	value
	)

Set observation model matrix value.

Parameters

ln	row index.
cl	column index.
value	output transition matrix value.

References DUNE::Math::Matrix::columns(), and DUNE::Math::Matrix::rows().

Referenced by Navigation::General::ROV::Task::resetKalman(), Navigation::AUV::Navigation::Task::resetKalman(), DUNE::Navigation::BasicNavigation::runKalmanLBL(), Navigation::General::ROV::Task::task(), and Navigation::AUV::Navigation::Task::task().

void DUNE::Navigation::KalmanFilter::setOutput	(	short	pos,
		double	value
	)

Set output matrix value.

Parameters

pos	matrix index.
value	output matrix value.

References DUNE::Math::Matrix::rows().

Referenced by DUNE::Navigation::BasicNavigation::runKalmanDVL(), Navigation::General::ROV::Task::runKalmanGPS(), Navigation::AUV::Navigation::Task::runKalmanGPS(), DUNE::Navigation::BasicNavigation::runKalmanLBL(), and Navigation::AUV::Navigation::Task::task().

void DUNE::Navigation::KalmanFilter::setProcessNoise	(	short	ln,
		short	cl,
		double	value
	)

Set process noise covariance matrix value.

Parameters

ln	row index.
cl	column index.
value	process noise covariance matrix value.

References DUNE::Math::Matrix::columns(), and DUNE::Math::Matrix::rows().

Referenced by Navigation::AUV::Navigation::Task::consume(), Navigation::General::ROV::Task::onUpdateParameters(), and Navigation::AUV::Navigation::Task::onUpdateParameters().

void DUNE::Navigation::KalmanFilter::setProcessNoise	(	short	in,
		double	value
	)

Set process noise covariance matrix value.

Parameters

in	row and column index.
value	process noise covariance matrix value.

void DUNE::Navigation::KalmanFilter::setProcessNoise

(

double

value

)

Set process noise covariance matrix value.

Parameters

value

process noise covariance matrix value.

void DUNE::Navigation::KalmanFilter::setState	(	short	pos,
		double	value
	)

Set state matrix value.

Parameters

pos	matrix index.
value	state matrix value.

Referenced by DUNE::Navigation::BasicNavigation::consume(), Navigation::AUV::Navigation::Task::consume(), Navigation::AUV::Navigation::Task::setup(), and Navigation::AUV::Navigation::Task::task().

void DUNE::Navigation::KalmanFilter::setStateTransition

(

Math::Matrix

a

)

Set state transition matrix.

Parameters

a	state transition matrix.

References DUNE::Math::Matrix::columns(), and DUNE::Math::Matrix::rows().

Referenced by setTransitions(), Navigation::General::ROV::Task::task(), and Navigation::AUV::Navigation::Task::task().

void DUNE::Navigation::KalmanFilter::setTransitions

(

Math::Matrix

a

)

Set transition matrices.

Parameters

a	state transition matrix.

References setCovarianceTransition(), and setStateTransition().

int DUNE::Navigation::KalmanFilter::update

(

float

threshold

)

Kalman Filter update function.

Parameters

threshold

threshold to reject large state innovations.

Returns

0 if update is successful, -1 otherwise.

References DUNE::Math::Matrix::columns(), and DUNE::Math::Matrix::rows().

Referenced by Navigation::General::ROV::Task::task(), and Navigation::AUV::Navigation::Task::task().

Collaboration diagram for DUNE::Navigation::KalmanFilter: