Model: \[x_k = A x_{k-1} + B u_k \] \[z_k = C x_k + v_k \]
Predict: \[\hat{x}_k = A \hat{x}_{k-1} + B u_k \] \[P_k = A P_{k-1} A^T \] Update: \[ G_k = P_k C^T (C P_k C^T + R)^{-1} \] \[ \hat{x}_k = \hat{x}_k + G_k(z_k - C \hat{x}_k) \] \[ P_k = (I - G_k C) P_k \] This seems like an awful lot of work just to be able to add a few extra items to our state variable! In fact, using linear algebra supports an extremely valuable capability of the Kalman Filter, called sensor fusion.
Returning to our airplane example, we note that pilots have access to much more information (observations) than just altitude: they also have gauges showing the plane's airspeed, groundspeed, heading, latitude and longitude, outside temperature, etc. Imagine a plane with just three sensors, each of which corresponds to a given part of the state: a barometer for altitude, a compass for heading, and a Pitot tube for airspeed. Assume for the time being that these sensors are perfectly accurate (no noise). Then our observation equation \[z_k = C x_{k-1} + v_k \] becomes \[ \begin{bmatrix} barometer_k\\ compass_k \\ pitot_k \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0\\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} altitude_{k-1}\\ heading_{k-1} \\ airspeed_{k-1} \end{bmatrix} \] Now imagine we have another sensor for altitude, say a GPS. Both the barometer and the GPS will be affected by altitude. So our equation becomes: \[ \begin{bmatrix} barometer_k\\ compass_k \\ pitot_k \\ gps_k \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0\\ 0 & 1 & 0 \\ 0 & 0 & 1 \\ 1 & 0 & 0 \end{bmatrix} \begin{bmatrix} altitude_{k-1}\\ heading_{k-1} \\ airspeed_{k-1} \end{bmatrix} \] Here we have our first example of a system without a simple one-to-one correspondence between sensors and state values. [14]. Any such system affords us with the opportunity for sensor fusion; that is, the ability to combine readings from more than one sensor (barometer, GPS) to infer something about a component (altitude) of the state.
As when we seek a second doctor's opinion on a medical condition, our intuition tells us that it is better to have more than one source of information about something important. In the next section, we will see how the Kalman Filter uses sensor fusion to give us a better state estimate than we can get with a one sensor alone.
Previous: Prediction and Update Revisited Next: Sensor Fusion Example
[14] Our $C$ matrix is somewhat unrealistic, in that the values would likely be something other than 1. The point is that a nonzero value in the matrix corresponds to a relationship between a sensor and a component of the state vector.