Kalman Filter
The KalmanFilter class implements a Kalman filter for linear systems. The filter can be used to estimate the state of a linear system given noisy measurements. The filter can be configured for an arbitrary number of states and measurements. You can also specify the process noise and measurement noise covariances.
To use the filter, you must first create an instance of the KalmanFilter class, then call the predict and update methods to estimate the state of the system. To get the current state estimate, call the get_state method.
API Reference
Header File
Classes
-
template<size_t N>
class KalmanFilter Kalman Filter for single variable state estimation
This class implements a simple Kalman filter for estimating the state of a variable, such as the orientation of a system. The filter is based on the following state-space model:
\[ X' = X + U \cdot dt \]\[ P' = P + Q \]\[ Y = Z - X \]\[ K = P / (P + R) \]\[ X = X + K \cdot Y \]\[ P = (1 - K) \cdot P \]where:\(X\) is the state vector
\(P\) is the covariance matrix
\(Q\) is the process noise
\(R\) is the measurement noise
\(U\) is the control input
\(dt\) is the time step
\(Z\) is the measurement
- Template Parameters:
N – Number of states
Public Functions
-
inline KalmanFilter()
Constructor.
-
inline void set_process_noise(float q)
Set process noise
Note
The process noise is the uncertainty in the model. It is used to compute the error covariance, which determines how much the prediction is trusted. A higher process noise means that the prediction is less trusted, and the filter relies more on the measurement. A lower process noise means that the prediction is more trusted, and the filter relies less on the measurement.
- Parameters:
q – Process noise
-
inline void set_process_noise(const std::array<float, N> &q)
Set process noise
- Parameters:
q – Process noise vector
-
inline void set_measurement_noise(float r)
Set measurement noise
Note
The measurement noise is the uncertainty in the measurement. It is used to compute the Kalman Gain, which determines how much the measurement should affect the state estimate.
- Parameters:
r – Measurement noise
-
inline void set_measurement_noise(const std::array<float, N> &r)
Set measurement noise
- Parameters:
r – Measurement noise vector
-
inline void predict(const std::array<float, N> &U, float dt)
Predict next state
Note
The prediction step estimates the state at the next time step based on the current state and the control input. The control input is used to model the effect of the control on the state. The time step is the time between measurements.
- Parameters:
U – Control input
dt – Time step
-
inline void update(const std::array<float, N> &Z)
Update state estimate
Note
The correction step updates the state estimate based on the measurement. The measurement is used to compute the Kalman Gain, which determines how much the measurement should affect the state estimate. The Kalman Gain is used to update the state estimate and the error covariance.
- Parameters:
Z – Measurement