DUNE: Uniform Navigational Environment
2.5.0-rc1
|
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) |
DUNE::Navigation::KalmanFilter::KalmanFilter | ( | void | ) |
Constructor.
DUNE::Navigation::KalmanFilter::KalmanFilter | ( | Math::Matrix & | A, |
Math::Matrix & | C, | ||
Math::Matrix & | P, | ||
Math::Matrix & | Q | ||
) |
Constructor.
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().
|
inline |
Get covariance matrix value.
ln | row index. |
cl | column index. |
References DUNE::Math::Matrix::columns(), and DUNE::Math::Matrix::rows().
Referenced by DUNE::Navigation::BasicNavigation::checkUncertainty(), Navigation::General::ROV::Task::logData(), Navigation::AUV::SGNavigation::Task::logData(), DUNE::Navigation::BasicNavigation::onDispatchNavigation(), Navigation::AUV::Navigation::Task::runKalmanLBL(), DUNE::Navigation::BasicNavigation::runKalmanLBL(), Navigation::AUV::Navigation::Task::task(), and Navigation::AUV::SGNavigation::Task::task().
|
inline |
Get covariance matrix value.
in | row and column index. |
|
inline |
Get state covariance matrix.
i1 | lower row index. |
i2 | upper row index. |
j1 | lower column index. |
j2 | upper column index. |
References DUNE::Math::Matrix::get().
|
inline |
Get state covariance matrix.
|
inline |
Get state covariance transition matrix.
|
inline |
Get innovation matrix value.
pos | matrix index. |
Referenced by Navigation::General::ROV::Task::logData(), and Navigation::AUV::SGNavigation::Task::logData().
|
inline |
Get observation model submatrix.
i1 | lower row index. |
i2 | upper row index. |
j1 | lower column index. |
j2 | upper column index. |
References DUNE::Math::Matrix::get().
Referenced by Navigation::AUV::Navigation::Task::runKalmanLBL().
|
inline |
Get output transition matrix.
|
inline |
Get output matrix value.
pos | matrix index. |
References DUNE::Math::Matrix::rows().
Referenced by Navigation::General::ROV::Task::task(), and Navigation::AUV::SGNavigation::Task::task().
|
inline |
Get filter state value.
pos | matrix index. |
Referenced by DUNE::Navigation::BasicNavigation::consume(), Navigation::AUV::SGNavigation::Task::getBiasedHeading(), Navigation::General::ROV::Task::logData(), Navigation::AUV::SGNavigation::Task::logData(), DUNE::Navigation::BasicNavigation::onDispatchNavigation(), Navigation::AUV::Navigation::Task::runKalmanGPS(), Navigation::AUV::SGNavigation::Task::setTransition(), Navigation::AUV::Navigation::Task::task(), Navigation::General::ROV::Task::task(), and Navigation::AUV::SGNavigation::Task::task().
|
inline |
Get state matrix.
|
inline |
Get state transition matrix.
void DUNE::Navigation::KalmanFilter::initialize | ( | Math::Matrix & | P0, |
Math::Matrix & | x0 | ||
) |
Set initial conditions (state and covariance matrix).
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.
b | control input matrix. |
u | input matrix. |
References DUNE::Math::Matrix::columns(), and DUNE::Math::Matrix::rows().
Referenced by Navigation::AUV::Navigation::Task::task(), Navigation::General::ROV::Task::task(), and Navigation::AUV::SGNavigation::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.
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::AUV::Navigation::Task::Task(), Navigation::General::ROV::Task::Task(), and Navigation::AUV::SGNavigation::Task::Task().
void DUNE::Navigation::KalmanFilter::resetCovariance | ( | short | in | ) |
Reset covariance values.
Referenced by Navigation::AUV::SGNavigation::Task::consume(), and Navigation::AUV::SGNavigation::Task::correctAlignment().
void DUNE::Navigation::KalmanFilter::resetOutputs | ( | void | ) |
Reset output matrices.
References DUNE::Math::Matrix::fill().
Referenced by Navigation::General::ROV::Task::resetKalman(), Navigation::AUV::SGNavigation::Task::resetKalman(), Navigation::AUV::Navigation::Task::runKalmanGPS(), and Navigation::AUV::Navigation::Task::runKalmanLBL().
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.
num_outputs | number of filter output states. |
References DUNE::Math::Matrix::resizeAndKeep(), and DUNE::Math::Matrix::rows().
Referenced by Navigation::General::ROV::Task::onConsumeLblConfig(), and Navigation::AUV::SGNavigation::Task::onConsumeLblConfig().
void DUNE::Navigation::KalmanFilter::setCovariance | ( | short | ln, |
short | cl, | ||
double | value | ||
) |
Set state covariance matrix value.
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::SGNavigation::Task::consume(), Navigation::AUV::SGNavigation::Task::correctAlignment(), Navigation::AUV::Navigation::Task::setup(), Navigation::General::ROV::Task::setup(), and Navigation::AUV::SGNavigation::Task::setup().
void DUNE::Navigation::KalmanFilter::setCovariance | ( | short | in, |
double | value | ||
) |
Set state covariance matrix value.
in | row and column index. |
value | state covariance matrix value. |
void DUNE::Navigation::KalmanFilter::setCovariance | ( | double | value | ) |
Set state covariance matrix value.
value | state covariance matrix value. |
void DUNE::Navigation::KalmanFilter::setCovarianceTransition | ( | Math::Matrix | a | ) |
Set state covariance transition matrix.
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::SGNavigation::Task::task().
void DUNE::Navigation::KalmanFilter::setInnovation | ( | short | pos, |
double | value | ||
) |
Set innovation matrix value.
pos | matrix index. |
value | innovation matrix value. |
References DUNE::Math::Matrix::rows().
Referenced by Navigation::AUV::Navigation::Task::runKalmanGPS(), Navigation::AUV::Navigation::Task::runKalmanLBL(), DUNE::Navigation::BasicNavigation::runKalmanLBL(), Navigation::General::ROV::Task::task(), and Navigation::AUV::SGNavigation::Task::task().
void DUNE::Navigation::KalmanFilter::setMeasurementNoise | ( | short | ln, |
short | cl, | ||
double | value | ||
) |
Set measurement noise covariance matrix value.
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::SGNavigation::Task::consume(), Navigation::AUV::Navigation::Task::onUpdateParameters(), Navigation::General::ROV::Task::onUpdateParameters(), Navigation::AUV::SGNavigation::Task::onUpdateParameters(), Navigation::AUV::Navigation::Task::runKalmanLBL(), and Navigation::AUV::SGNavigation::Task::updateKalmanParametersGps().
void DUNE::Navigation::KalmanFilter::setMeasurementNoise | ( | short | in, |
double | value | ||
) |
Set measurement noise covariance matrix value.
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.
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.
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::SGNavigation::Task::resetKalman(), Navigation::AUV::Navigation::Task::runKalmanGPS(), Navigation::AUV::Navigation::Task::runKalmanLBL(), DUNE::Navigation::BasicNavigation::runKalmanLBL(), Navigation::General::ROV::Task::task(), and Navigation::AUV::SGNavigation::Task::task().
void DUNE::Navigation::KalmanFilter::setOutput | ( | short | pos, |
double | value | ||
) |
Set output matrix value.
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::SGNavigation::Task::runKalmanGPS(), DUNE::Navigation::BasicNavigation::runKalmanLBL(), and Navigation::AUV::SGNavigation::Task::task().
void DUNE::Navigation::KalmanFilter::setProcessNoise | ( | short | ln, |
short | cl, | ||
double | value | ||
) |
Set process noise covariance matrix value.
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::SGNavigation::Task::consume(), Navigation::General::ROV::Task::onUpdateParameters(), Navigation::AUV::Navigation::Task::onUpdateParameters(), Navigation::AUV::SGNavigation::Task::onUpdateParameters(), and Navigation::AUV::Navigation::Task::task().
void DUNE::Navigation::KalmanFilter::setProcessNoise | ( | short | in, |
double | value | ||
) |
Set process noise covariance matrix value.
in | row and column index. |
value | process noise covariance matrix value. |
void DUNE::Navigation::KalmanFilter::setProcessNoise | ( | double | value | ) |
Set process noise covariance matrix value.
value | process noise covariance matrix value. |
void DUNE::Navigation::KalmanFilter::setState | ( | short | pos, |
double | value | ||
) |
Set state matrix value.
pos | matrix index. |
value | state matrix value. |
Referenced by DUNE::Navigation::BasicNavigation::consume(), Navigation::AUV::SGNavigation::Task::consume(), Navigation::AUV::SGNavigation::Task::correctAlignment(), Navigation::AUV::Navigation::Task::setup(), Navigation::AUV::SGNavigation::Task::setup(), and Navigation::AUV::Navigation::Task::task().
void DUNE::Navigation::KalmanFilter::setStateTransition | ( | Math::Matrix | a | ) |
Set state transition matrix.
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::SGNavigation::Task::task().
void DUNE::Navigation::KalmanFilter::setTransitions | ( | Math::Matrix | a | ) |
Set transition matrices.
a | state transition matrix. |
References setCovarianceTransition(), and setStateTransition().
Referenced by Navigation::AUV::Navigation::Task::task().
int DUNE::Navigation::KalmanFilter::update | ( | float | threshold | ) |
Kalman Filter update function.
threshold | threshold to reject large state innovations. |
References DUNE::Math::Matrix::columns(), and DUNE::Math::Matrix::rows().
Referenced by Navigation::AUV::Navigation::Task::runKalmanGPS(), Navigation::AUV::Navigation::Task::runKalmanLBL(), Navigation::General::ROV::Task::task(), and Navigation::AUV::SGNavigation::Task::task().