 |
Robotics Library
0.7.0
|
|
Robotics Library
0.7.0
|
#include <Kalman.h>
Public Types | |
| typedef Scalar | ScalarType |
| typedef ::Eigen::Matrix< Scalar, ::Eigen::Dynamic, ::Eigen::Dynamic > | MatrixType |
| typedef ::Eigen::Matrix< Scalar, ::Eigen::Dynamic, 1 > | VectorType |
Private Attributes | |
| MatrixType | A |
| \(\matr{A}\) relates the state at the previous time step \(k - 1\) to the state at the current step \(k\). More... | |
| MatrixType | B |
| \(\matr{B}\) relates the control input \(\vec{u}\) to the state \(\vec{x}\). More... | |
| MatrixType | H |
| \(\matr{H}\) relates the state to the measurement \(\vec{z}_{k}\). More... | |
| MatrixType | PPosteriori |
| A posteriori estimate error covariance \(\matr{P}_{k - 1}\). More... | |
| MatrixType | PPriori |
| A priori estimate error covariance \(\matr{P}^{-}_{k}\). More... | |
| MatrixType | Q |
| Process noise covariance \(\matr{Q}\). More... | |
| MatrixType | R |
| Measurement error covariance \(\matr{R}\). More... | |
| VectorType | xPosteriori |
| A posteriori state estimate \(\hat{\vec{x}}_{k - 1}\). More... | |
| VectorType | xPriori |
| A priori state estimate \(\hat{\vec{x}}^{-}_{k}\). More... | |
Kalman filter.
Greg Welch and Gary Bishop. An introduction to the Kalman filter. Technical Report TR 95-041, University of North Carolina at Chapel Hill, Chapel Hill, NC, USA, July 2006.
| typedef ::Eigen::Matrix<Scalar, ::Eigen::Dynamic, ::Eigen::Dynamic> rl::math::Kalman< Scalar >::MatrixType |
| typedef Scalar rl::math::Kalman< Scalar >::ScalarType |
| typedef ::Eigen::Matrix<Scalar, ::Eigen::Dynamic, 1> rl::math::Kalman< Scalar >::VectorType |
|
inline |
|
inlinevirtual |
|
inline |
|
inline |
|
inline |
Measurement update ("correct").
Compute the Kalman gain
\[ \matr{K}_{k} = \matr{P}^{-}_{k} \matr{H}^{\mathrm{T}} \left( \matr{H} \matr{P}^{-}_{k} \matr{H}^{\mathrm{T}} + \matr{R} \right)^{-1} \]
Update estimate with measurement \(\vec{z}_{k}\)
\[ \hat{\vec{x}}_{k} = \hat{\vec{x}}^{-}_{k} + \matr{K}_{k} \left( \vec{z}_{k} - \matr{H} \hat{\vec{x}}^{-}_{k} \right) \]
Update the error covariance
\[ \matr{P}_{k} = \left( \matr{1} - \matr{K}_{k} \matr{H} \right) \matr{P}^{-}_{k} \]
| [in] | z | Measurement \(\vec{z}_{k}\) |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
Time update ("predict") without control input.
Project the state ahead
\[ \hat{\vec{x}}^{-}_{k} = \matr{A} \hat{\vec{x}}_{k - 1} \]
Project the error covariance ahead
\[ \matr{P}^{-}_{k} = \matr{A} \matr{P}_{k - 1} \matr{A}^{\mathrm{T}} + \matr{Q} \]
|
inline |
Time update ("predict") with control input.
Project the state ahead
\[ \hat{\vec{x}}^{-}_{k} = \matr{A} \hat{\vec{x}}_{k - 1} + \matr{B} \vec{u}_{k - 1} \]
Project the error covariance ahead
\[ \matr{P}^{-}_{k} = \matr{A} \matr{P}_{k - 1} \matr{A}^{\mathrm{T}} + \matr{Q} \]
| [in] | u | Control input \(\vec{u}_{k - 1}\) |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
|
private |
\(\matr{A}\) relates the state at the previous time step \(k - 1\) to the state at the current step \(k\).
|
private |
\(\matr{B}\) relates the control input \(\vec{u}\) to the state \(\vec{x}\).
|
private |
\(\matr{H}\) relates the state to the measurement \(\vec{z}_{k}\).
|
private |
A posteriori estimate error covariance \(\matr{P}_{k - 1}\).
|
private |
A priori estimate error covariance \(\matr{P}^{-}_{k}\).
|
private |
Process noise covariance \(\matr{Q}\).
|
private |
Measurement error covariance \(\matr{R}\).
|
private |
A posteriori state estimate \(\hat{\vec{x}}_{k - 1}\).
|
private |
A priori state estimate \(\hat{\vec{x}}^{-}_{k}\).
1.8.20