Kalman Filter

Kalman Filters are used for state estimation in control systems when noisy measurements are present. This repository includes an implementation of the algorithm in Python and also a Jupyter Notebook for testing in real data for altitude estimation of a quadrotor

Kalman Algorithm

Initialization

0. Start

1. Initialize the state estimate: $\hat{x}_{0|0}$ and the error covariance estimate: $P_{0|0}$

Prediction Step

2. Predict the state for the next time step:
   $\hat{x}_{k|k-1} = A_k \hat{x}_{k-1|k-1} + B_k u_k$

3. Predict the error covariance for the next time step:
   $P_{k|k-1} = A_k P_{k-1|k-1} A_k^T + Q_k$

Correct Step

4. Compute the Kalman Gain:
   $K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1}$

5. Update the state estimate:
   $\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k(z_k - H_k \hat{x}_{k|k-1})$

6. Update the error covariance estimate:
   $P_{k|k} = (I - K_k H_k)P_{k|k-1}$

   Go to 2

The matrices and vectors in the above equations are defined as follows:

• $A_k$: State transition matrix for time step $k$
• $B_k$: Input control matrix for time step $k$
• $u_k$: Control input at time step $k$
• $Q_k$: Process noise covariance matrix at time step $k$
• $H_k$: Measurement matrix at time step $k$
• $R_k$: Measurement noise covariance matrix at time step $k$
• $z_k$: Measurement vector at time step $k$
• $I$: Identity matrix