Kalman Filter FrameWork - Welcome to KF
SLAM is a state estimation problem. A typical state estimation framework in discrete time is:
\[\begin{gather*} \begin{cases} x_k = f(x_{k-1}, u_k) + \mathbf{w}_k, \quad k = 1, \dots, N \\ z_k = h(x_k) + \mathbf{v}_k \end{cases} \end{gather*}\]Where
- f is the state transition function, and h is the measurement (observation) function.
- \(v_k ~ N(0, Q_k)\) and \(w_k ~ N(0, R_k)\) are Gaussian noises.
- \(u_k\) is the “control signal”, \(z_k\) is the “observation”.
When f and h are linear, that is, we have a linear system, our state estimation framework becomes:
\[\begin{gather*} \begin{cases} x_k = A_{k} x_{k-1} + u_{k} + w_{k} \\ z_k = C_{k} x_{k} + v_{k} \end{cases} \end{gather*}\]Then, The framwork is as follows:
- When a new control update \(u_k\) comes in, we can calculate a prediction of x, \(x_k^*\), the prediction covariance matrix, \(P_k^*\), and the Kalman gain \(K_k\)
- Note that in this step, the covariance matrix \(P_k^*\) should increase from \(P_{k-1}\), because we are updating with the control signal and we don’t have the feedback from observation yet.
- When a new observation comes in, we can calculate our posteriors. Note, here \(P_k\) is the final estimate of the covariance matrix
- Where \(z_k - C_k x_k^*\) is called “innovation”, meaning the “new information learned from the observation”
Variable Dimensions
Vectors:
- \(x_k\):
[m, 1] - \(u_k\):
[m, m] - \(z_k\):
[n, 1]
Correspondingly, the matrices are:
- \(A_k\):
[m, m] - \(P_k\):
[m, m] - \(C_k\):
[n, m] - \(K_k\):
[m, n] - \(R_k\):
[m, m] - \(Q_k\):
[n. n]
Proof 1 - Minimize State Estimation Covariance From Ground Truth (Most Common Proof)
The goal of Kalman filter is to minimize the state estimation covariance from ground truth \(x_{k, true}\):
\[\begin{gather*} \begin{aligned} & min E[(x_{k, true} - x_{k})(x_{k, true} - x_{k})^T] \\ & \text{Given: } \\ & z_k = H_k x_{k, true} + v_k \\ & \rightarrow \\ & e_k = (x_{k, true} - x_{k}) = x_{k, true} - (x_k^* + K_k(z_k - C_k x_k^*)) \\ & = x_{k, true} - (x_k^* + K_k(H_k x_{k, true} + v_k - C_k x_k^*)) \\ & = (I - K_k H_k)(x_{k, true} - x_k^*) - K_kv_k \end{aligned} \end{gather*}\]Then, the above can be written as:
\[\begin{gather*} \begin{aligned} & E[(x_{k, true} - x_{k})(x_{k, true} - x_{k})^T] = E(e_ke_k^T) \\ & = E[((I - K_k H_k)(x_{k, true} - x_k^*) - K_kv_k) ((I - K_k H_k)(x_{k, true} - x_k^*) - K_kv_k)^T] \\ & = E[(I - K_k H_k)(x_{k, true} - x_k^*)(x_{k, true} - x_k^*)^T(I - K_k H_k)^T] + E[(K_kv_k)(K_kv_k)^T] \\ & -E[(I - K_k H_k)(x_{k, true} - x_k^*)(K_kv_k)^T] - E[(K_kv_k)(x_{k, true} - x_k^*)^T(I - K_k H_k)^T] \end{aligned} \end{gather*}\]General Form of \(P_k\)
Since \(v_k\) and \(x_k\) are independent, cross terms are zero. So we can derive the general form of P_k:
\[\begin{gather*} \begin{aligned} & P_k := E[(x_{k, true} - x_{k})(x_{k, true} - x_{k})^T] \\ & = E[(I - K_k H_k)(x_{k, true} - x_k^*)(x_{k, true} - x_k^*)^T(I - K_k H_k)^T] + E[(K_kv_k)(K_kv_k)^T] \\ & = (I - K_k H_k)P_{k}^*(I - K_k H_k)^T + K_k R K_k^T \end{aligned} \end{gather*}\]Prediction of covariance \(P_{k}^*\)
For equantion(2) a-priori error covariance is:
\[\begin{gather*} \begin{aligned} & P_{k}^* := E[(x_{k, true} - x_k^*)(x_{k, true} - x_k^*)^T] \\ & \text{define:} \\ & e_k^* = (x_{k, true} - x_k) = A(x_{k-1, true} - x_{k-1}) + w_k \\ & P_{k}^* = E[(A(x_{k-1, true} - x_{k-1}) + w_k) (A(x_{k-1, true} - x_{k-1}) + w_k)^T] \\ & = E[A P_{k-1} A^T] + E[w_k w_k^T] \\ & = A P_{k-1} A^T + Q \end{aligned} \end{gather*}\]Kalman Gain \(K_k\) and Covariance \(P_k\)
When the covariance \(P_k\) is the smallest, its partial derivative w.r.t \(K_k\) should be 0
\[\begin{gather*} \begin{aligned} & \frac{\partial P_k}{\partial K_k} = 0 \\ & \rightarrow \\ & \frac{\partial}{\partial K_k} \left[ (I - K_k C_k) P_k^* (I - K_k C_k)^T \right] = -C_k P_k^* (I - K_k C_k)^T - (I - K_k C_k) P_k^* C_k^T \\ & \end{aligned} \end{gather*}\]Also, the other derivative
\[\begin{gather*} \begin{aligned} & \frac{\partial}{\partial K_k} \left[ K_k R K_k^T \right] = K_k R + R K_k^T \\ \end{aligned} \end{gather*}\]Note that \(R = R^T\), \(P_k^* = (P_k^*)^T\), \(P_k = (P_k)^T\)
Combine both results:
\[\begin{gather*} \begin{aligned} & \frac{\partial P_{k}}{\partial K_k} = 2(-C_k P_k^* + K_k C_k P_k^* C_k^T + K_k R) = 0 \\ & \rightarrow C_k P_k^* = K_k \left( C_k P_k^* C_k^T + R \right) \\ & \rightarrow K_k = P_k^* C_k^T \left( C_k P_k^* C_k^T + R \right)^{-1} \end{aligned} \end{gather*}\]Without proof, when K_k is optimal, \(P_k = (I - K_k C_k) P_k^*\).
Proof 2 - MAP (Maximum A-Posteriori)
For a refresher of MAP, please see here
In a linear system,
\[\begin{gather*} \begin{aligned} & x_k = A_k x_{k-1} + u_k + w_k \\ & z_k = C_{k} x_{k} + v_{k} \end{aligned} \end{gather*}\]Here, we assume:
- \(x_k\), \(x_{k-1}\) are ground truth
- control noise \(w_k \sim \mathcal{N}(0, Q)\),
- observation \(v_k sim \mathcal{N}(0, R)\)
- each state vector \(x_k\) has a covariance matrix \(P_k\)
So according to the linear transforms of Multivariate Gaussian Distribution, we can write the joint distribution:
\[\begin{gather*} \begin{aligned} & P(x_k | x_{k-1}) \sim \mathcal{N}(A_k x_{k-1}, A_k^T P_{k-1} A_k + Q) \\ & P(z_k | x_k) \sim \mathcal{N}(C_k x_{k}, R) \end{aligned} \end{gather*}\]- Note that the noise covariance \(z_k\) is independent from \(x_k\), that’s why we have \(P(z_k \vert x_k) \sim \mathcal{N}(C_k x_{k}, R)\)
In the MAP framework, we are interested in finding the posterior \(\hat{x_k}\) (estimate of \(x_k\)) using the log trick on multivariate Gaussian distribution:
\[\begin{gather*} \begin{aligned} & \hat{x_k}, P_k = argmax [\mathcal{N}(C_k x_{k}, R) \mathcal{N} (A_k x_{k-1}, A_k^T P_{k-1} A_k + Q)] \\ & \text{using the log trick: } \\ & \Rightarrow \hat{x_k}, P_k = argmin[J_k] = argmin [(z_k - C_k x_{k})^T R^{-1} (z_k - C_k x_{k}) + (x_k - A_k x_{k-1})^T (A_k^T P_{k-1} A_k + R)^{-1} (x_k - A_k x_{k-1})] \end{aligned} \end{gather*}\]Let
\[\begin{gather*} \begin{aligned} & P_k^* = A_k^T P_{k-1} A_k + R \\ & \Rightarrow argmin[J_k] = argmin [(z_k - C_k x_{k})^T R^{-1} (z_k - C_k x_{k}) + (x_k - A_k x_{k-1})^T (P_k^*)^{-1} (x_k - A_k x_{k-1})] \end{aligned} \end{gather*}\]Now, let’s do the heavy lifting: differentiating w.r.t our variable of interest, \(x_k\), and solve it while it’s 0:
\[\begin{gather*} \begin{aligned} & \frac{\partial J_k}{\partial x_k} = -2 C_k^T R^{-1} (z_k - C_k x_{k}) + 2(P_k^*)^{-1} (x_k - A_k x_{k-1}) = 0 \\ & \Rightarrow \mathbf{C}_k^\top \mathbf{R}^{-1} \mathbf{z}_k - \mathbf{C}_k^\top \mathbf{R}^{-1} \mathbf{C}_k \mathbf{x}_k + (\mathbf{P}_k^{*})^{-1} \mathbf{x}_k - (\mathbf{P}_k^{*})^{-1} \hat{\mathbf{x}}_k^{*} = 0 \\ & \Rightarrow (\mathbf{C}_k^\top \mathbf{R}^{-1} \mathbf{C}_k + (\mathbf{P}_k^{*})^{-1}) \mathbf{x}_k = \mathbf{C}_k^\top \mathbf{R}^{-1} \mathbf{z}_k + (\mathbf{P}_k^{*})^{-1} \hat{\mathbf{x}}_k^{*} \\ & \Rightarrow \mathbf{x}_k = (\mathbf{C}_k^\top \mathbf{R}^{-1} \mathbf{C}_k + (\mathbf{P}_k^{*})^{-1})^{-1} (\mathbf{C}_k^\top \mathbf{R}^{-1} \mathbf{z}_k + (\mathbf{P}_k^{*})^{-1} \hat{\mathbf{x}}_k^{*}) \end{aligned} \end{gather*}\]We are skipping another leavy lift: getting the covariance of \(x_k\), \(P_k\). If you are curious, the covariance of a multivariate Gaussian distribution can be achieved by taking the Hessian of its negative log function.
\[\begin{gather*} \begin{aligned} & P_k = ((P_k^{*})^{-1} + C_k^T R^{-1} C_k)^{-1} \end{aligned} \end{gather*}\]By using the Woodbury matrix identity
\[\begin{gather*} \begin{aligned} & P_k = P_k^{*} - P_k^{*} C_k^{T}(R^{-1} + C_k P_k^{*} C_k^T)^{-1} C_k P_k^{*} \end{aligned} \end{gather*}\]Let
\[\begin{gather*} \begin{aligned} & K_k = P_k^{*} C_k^{T}(R^{-1} + C_k P_k^{*} C_k^T) \\ & \rightarrow P_k = P_k^{*} - K_k C_k P_k^{*} \end{aligned} \end{gather*}\]\(x_k\) can be simplified to:
\[\begin{gather*} \begin{aligned} & \mathbf{x}_k = (P_k^{*} - K_k C_k P_k^{*}) (\mathbf{C}_k^\top \mathbf{R}^{-1} \mathbf{z}_k + (\mathbf{P}_k^{*})^{-1} \hat{\mathbf{x}}_k^{*}) \\ & = A_k x_{k-1} − K_k C_k A_k x_{k-1} + K_k z_k = (x^* - K_k C_k x^* + K_k z_k) \end{aligned} \end{gather*}\]So we can see that we’ve derived the covariance matrix \(P_k\), Kalman Gain \(K_k\), and the final estimate \(x_k\)
EKF (Extended Kalman Filter)
EKF is applied where the system is non-linear:
\[\begin{gather*} \begin{cases} x_k = f(x_{k-1}, u_k) + \mathbf{w}_k, \quad k = 1, \dots, N \\ z_k = h(x_k) + \mathbf{v}_k \end{cases} \end{gather*}\]Our important intermediate variables are:
\[\begin{gather*} \begin{aligned} & P_k^* = A_k^T P_{k-1} A_k + R \Rightarrow P_k^* = \frac{\partial f}{\partial x} P_{k-1} \frac{\partial f^T}{\partial x} + R \\ & K_k = P_k^{*} C_k^{T}(R^{-1} + C_k P_k^{*} C_k^T) \Rightarrow K_k = P_k^{*} \frac{\partial h}{\partial x}^{T} (R^{-1} + \frac{\partial h}{\partial x} P_k^{*} \frac{\partial h}{\partial x}^T) \end{aligned} \end{gather*}\]Finally our updates are:
\[\begin{gather*} \begin{aligned} & x_k = x_k^* + K_k(z_k - C_k x_k^*) \Rightarrow x_k = x_{k-1} + K(z_t - h(x_k^*)) \\ & P_k = P_k^{*} - K_k C_k P_k^{*} \Rightarrow P_k = P_k^{*} - K_k \frac{\partial h}{\partial x} P_k^{*} \end{aligned} \end{gather*}\]The first order derivative can be achieved through Jacobians
