|
| template<int AOptions_ = 0, int AMaxRows_ = NStates, int AMaxCols_ = NStates, int HOptions_ = 0, int HMaxRows_ = NMeasurements, int HMaxCols_ = NStates, int BOptions_ = 0, int BMaxRows_ = NStates, int BMaxCols_ = NInputs, int ROptions_ = 0, int RMaxRows_ = NMeasurements, int RMaxCols_ = NMeasurements, int POptions_ = 0, int PMaxRows_ = NStates, int PMaxCols_ = NStates, int QOptions_ = 0, int QMaxRows_ = NStates, int QMaxCols_ = NStates> |
| | KalmanFilter (const Eigen::Matrix< T, NStates, NStates, AOptions_, AMaxRows_, AMaxCols_ > &A, const Eigen::Matrix< T, NMeasurements, NStates, HOptions_, HMaxRows_, HMaxCols_ > &H, const Eigen::Matrix< T, NStates, NInputs, BOptions_, BMaxRows_, BMaxCols_ > &B=Eigen::Matrix< T, NStates, NInputs >::Zero(), const Eigen::Matrix< T, NMeasurements, NMeasurements, ROptions_, RMaxRows_, RMaxCols_ > &R=Eigen::Matrix< T, NMeasurements, NMeasurements >::Identity(), const Eigen::Matrix< T, NStates, NStates, POptions_, PMaxRows_, PMaxCols_ > &P_0=Eigen::Matrix< T, NStates, NStates >::Identity() *static_cast< T >(1e+3), const Eigen::Matrix< T, NStates, NStates, QOptions_, QMaxRows_, QMaxCols_ > &Q=Eigen::Matrix< T, NStates, NStates >::Identity() *static_cast< T >(1e-2), const Eigen::Vector< T, NStates > &x_0=Eigen::Vector< T, NStates >::Zero()) |
| | Initialises a Kalman filter.
|
| |
| template<int AOptions_ = 0, int AMaxRows_ = NStates, int AMaxCols_ = NStates> |
| KalmanFilter & | setA (const Eigen::Matrix< T, NStates, NStates, AOptions_, AMaxRows_, AMaxCols_ > &new_A) |
| | Set the system matrix.
|
| |
| template<int AOptions_ = 0, int AMaxRows_ = NStates, int AMaxCols_ = NStates> |
| KalmanFilter & | setSystemMatrix (const Eigen::Matrix< T, NStates, NStates, AOptions_, AMaxRows_, AMaxCols_ > &new_A) |
| | Set the system matrix.
|
| |
| template<int HOptions_ = 0, int HMaxRows_ = NMeasurements, int HMaxCols_ = NStates> |
| KalmanFilter & | setH (const Eigen::Matrix< T, NMeasurements, NStates, HOptions_, HMaxRows_, HMaxCols_ > &new_H) |
| | Sets the observation matrix.
|
| |
| template<int HOptions_ = 0, int HMaxRows_ = NMeasurements, int HMaxCols_ = NStates> |
| KalmanFilter & | setObservationMatrix (const Eigen::Matrix< T, NMeasurements, NStates, HOptions_, HMaxRows_, HMaxCols_ > &new_H) |
| | Sets the observation matrix.
|
| |
| template<int BOptions_ = 0, int BMaxRows_ = NStates, int BMaxCols_ = NInputs> |
| KalmanFilter & | setB (const Eigen::Matrix< T, NStates, NInputs, BOptions_, BMaxRows_, BMaxCols_ > &new_B) |
| | Sets the control input matrix.
|
| |
| template<int BOptions_ = 0, int BMaxRows_ = NStates, int BMaxCols_ = NInputs> |
| KalmanFilter & | setControlInputMatrix (const Eigen::Matrix< T, NStates, NInputs, BOptions_, BMaxRows_, BMaxCols_ > &new_B) |
| | Sets the control input matrix.
|
| |
| template<int ROptions_ = 0, int RMaxRows_ = NMeasurements, int RMaxCols_ = NMeasurements> |
| KalmanFilter & | setR (const Eigen::Matrix< T, NMeasurements, NMeasurements, ROptions_, RMaxRows_, RMaxCols_ > &new_R) |
| | Sets the measurement covariance matrix.
|
| |
| template<int ROptions_ = 0, int RMaxRows_ = NMeasurements, int RMaxCols_ = NMeasurements> |
| KalmanFilter & | setMeasurementCovMatrix (const Eigen::Matrix< T, NMeasurements, NMeasurements, ROptions_, RMaxRows_, RMaxCols_ > &new_R) |
| | Sets the measurement covariance matrix.
|
| |
| template<int POptions_ = 0, int PMaxRows_ = NStates, int PMaxCols_ = NStates> |
| KalmanFilter & | setP (const Eigen::Matrix< T, NStates, NStates, POptions_, PMaxRows_, PMaxCols_ > &new_P) |
| | Sets the error covariance matrix.
|
| |
| template<int POptions_ = 0, int PMaxRows_ = NStates, int PMaxCols_ = NStates> |
| KalmanFilter & | setErrorCovMatrix (const Eigen::Matrix< T, NStates, NStates, POptions_, PMaxRows_, PMaxCols_ > &new_P) |
| | Sets the error covariance matrix.
|
| |
| template<int QOptions_ = 0, int QMaxRows_ = NStates, int QMaxCols_ = NStates> |
| KalmanFilter & | setQ (const Eigen::Matrix< T, NStates, NStates, QOptions_, QMaxRows_, QMaxCols_ > &new_Q) |
| | Sets the process noise covariance matrix.
|
| |
| template<int QOptions_ = 0, int QMaxRows_ = NStates, int QMaxCols_ = NStates> |
| KalmanFilter & | setProcessNoiseCovMatrix (const Eigen::Matrix< T, NStates, NStates, QOptions_, QMaxRows_, QMaxCols_ > &new_Q) |
| | Sets the process noise covariance matrix.
|
| |
| template<int POptions_ = 0, int PMaxRows_ = NStates, int PMaxCols_ = NStates> |
| KalmanFilter & | setx (const Eigen::Vector< T, NStates > &new_x) |
| | Sets the state vector.
|
| |
| template<int POptions_ = 0, int PMaxRows_ = NStates, int PMaxCols_ = NStates> |
| KalmanFilter & | setStateVector (const Eigen::Vector< T, NStates > &new_x) |
| | Sets the state vector.
|
| |
| Eigen::Vector< T, NStates > | makePrediction (const Eigen::Vector< T, NInputs > &u) const |
| | predicts the next system state
|
| |
| void | add (const Eigen::Vector< T, NMeasurements > &z, const Eigen::Vector< T, NInputs > &u=Eigen::Vector< T, NInputs >::Zero()) |
| |
template<std::same_as< T > U>
requires (NMeasurements == 1 && NInputs != 1) |
| void | add (const U &z, const Eigen::Vector< U, NInputs > &u=Eigen::Vector< U, NInputs >::Zero()) |
| | This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.
|
| |
template<std::same_as< T > U>
requires (NMeasurements != 1 && NInputs == 1) |
| void | add (const Eigen::Vector< U, NMeasurements > &z, const U &u=static_cast< T >(0)) |
| | This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.
|
| |
template<std::same_as< T > U>
requires (NMeasurements == 1 && NInputs == 1) |
| void | add (const U &z, const U &u=static_cast< T >(0)) |
| | This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.
|
| |
| const Eigen::Vector< T, NStates > & | estimate () const |
| | returns the current best estimate of the system states
|
| |
| const Eigen::Matrix< T, NStates, NStates > & | errorCovariance () const |
| | returns the error covariance matrix
|
| |
| void | reset (const Eigen::Vector< T, NStates > &x_0=Eigen::Vector< T, NStates >::Zero(), const Eigen::Matrix< T, NStates, NStates > &P_0=Eigen::Matrix< T, NStates, NStates >::Identity() *static_cast< T >(1e+3)) |
| | Resets the state vector and error covariance matrix.
|
| |
template<class T, int NStates, int NMeasurements = 1, int NInputs = 0>
class controlpp::KalmanFilter< T, NStates, NMeasurements, NInputs >
Kalman filter.
The Kalman filter is an efficient, recursive algorithm for estimating the internal state of a dynamic system from a series of noisy, incomplete measurements. It combines predictions from a mathematical model with real-time sensor data to produce an estimate that is statistically optimal under certain assumptions.
- Predict state and error covariance
\[
\hat{x}_{k} = \mathbf{A} x_{k-1}\\
\hat{\mathbf{P}}_{k} = \mathbf{A} \mathbf{P}_{k-1} \mathbf{A}^{\top} + \mathbf{Q}\\
\]
- Calculate Kalman Gain
\[
\mathbf{K}_{k} = \mathbf{P}_{k-1} \mathbf{H}^{\top} \left( \mathbf{H} \mathbf{P}_{k-1} \mathbf{H}^{\top} + \mathbf{R} \right)^{-1}
\]
- Update state estimate and error covariance
\[
x_{k} = \hat{x}_{k} + \mathbf{K}_{k} \left( z_k - \mathbf{H} \hat{x}_{k} \right)\\
\mathbf{P}_{k} = (I - \mathbf{K}_{k} \mathbf{H}) \hat{\mathbf{P}}_{k}
\]
with:
- \(\mathbf{A}\) The system matrix, aka. state transition matrix
You get it from the systems identification/modeling/transfer function/state space
- \(\mathbf{B}\) The control input matrix
You get if from the systems identification/modeling/transfer function/state space
- \(\mathbf{H}\) The observation matrix
The observation matrix maps the state vector x to the measuremnts z:
\[
z_k = \mathbf{H} x_k
\]
\(\mathbf{R}\) The measurement noise covariance matrix
Captures the uncertainty in the measurement z.
Diagonal elements represent the variance (= squared standar deviation) of the measurement noise.
- Large diagonal elements \(\Rightarrow\) higher trust in model
- Small diagonal elements \(\Rightarrow\) higher trust in measurements
Off diagonal elements tell the filter how measurements are correlated, for example when two sensors depend on the same noise or are coupled.
- Higher off-diagonal elements \(\Rightarrow\) higher coupling
- Zero off-diagonal elements \(\Rightarrow\) no coupling
\(\mathbf{Q}\) The process noise covariance matrix
Models imperfections in the system model and assumes additive gaussian noise on the state prediction step. It is used to:
- model uncertainties in the prediction
- account for model simplifications
- unmodeled dynamics
diagonal entries reflect how uncertain you are about the state variable:
- higher diagonal elements \(\Rightarrow\) more uncertainty. Model can't reliably predict this state, let measurements correct it.
- lower diagonal elements \(\Rightarrow\) less uncertainty. The filter will trust the models prediction more.
\(\mathbf{P}\) The error covariance matrix Represents the uncertainty of the Kalman's filters estimate x.
Diagonal elements represent the variance of the state variable x:
- Large diagonal elements \(\Rightarrow\) the filter is uncertain about the corresponding state
- Small diagonal elements \(\Rightarrow\) the filter is confident about the value of the corresponding state
Off-Diagonal elements estimate correlations between states:
- Non-Zero \(\Rightarrow\) those states are correlated
- Zero \(\Rightarrow\) those states are uncorrelated
For Initialisation P_0 should reflect how much you trust your initial guess of x_0:
- diagonal elements smaller 1: you are quite confident about the state
- diagonal elements larger 1: you are unsure about the initial guess and sensor readings should correct it quite quickly
- much larger than 1 (e.g.: 1000): The initial guess is completely unreliable the filter should deduce it all from measurements.
- \(\hat{\mathbf{P}}\) The predicted error covariance matrix
- \(x\) The estimated state vector
- \(\hat{x}\) The predicted estimated state vector
1.9.8