What is Kalman Filter?
Kalman filter is a method to estimate true state from data including uncertainty. For example, the Kalman filter was used in the rocket of the Apollo program. All sensors have error in their measurement value, so we need to correct them to estimate the true state value.
Kalman filter uses odometry and observation to estimate true state. We have to determine Kalman gain which balance the importance of odometry and observation.
Equations
State equation of a system can be described as below.
\[x_{k} = A_{k}x_{k-1} + B_{k}u_{k} + w_{k}\]
where
\(x_{k}\) is state at \(k\), \(A_{k}\) is state transition matrix, \(u_{k}\) is an input, and \(w_{k}\) is a noise.
\[w_{k} \sim \mathcal{N}(0,Q)\]
Observation
\[y_{k} = C_{k}x_{k} + v_{k}\]
where \(y_{k}\) is observation at time \(k\). and \(v_{k}\) is a noise.
\[v_{k} \sim \mathcal{N}(0,R)\]
Predict
\[x_{k|k-1} = F_{k}x_{k-1|k-1} + B_{k}u_{k}\]
\[P_{k|k-1} = F_{k}P_{k-1|k-1}F^{T}_{k} + G_{k}Q_{k}G^{T}_{k}\]
Update
\[e_{k} = z_{k} – H_{k}\hat{x}_{k|k-1}\]
\[S_{k} = R_{k} + H_{k}P_{k|k-1}H^{T}_{k}\]
\[K_{k} = P_{k|k-1}H^{T}_{k}S^{-1}_{k}\]
\[\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_{k}e_{k}\]
\[P_{k|k} = (I-K_{k}H_{k})P_{k|k-1}\]
Estimate
\[x_{k|k} = (I-K_{k}H_{k})\hat{x}_{k|k-1} + K_{k}z_{k}\]