Kalman Filter

The convergence rate of the Kalman filter is relatively fast, but the implementation is more complex than that of LMS-based algorithms.

The Kalman filter is a linear optimum filter that minimizes the mean of the squared error recursively.

Recall that the equation J(k) = E[e²(k)] defines the cost function. The following procedure lists the steps of the Kalman filter algorithm.

1. Initialize the parametric vector
   $\stackrel{\rightharpoonup }{w}\left(k+1\right)=\stackrel{\rightharpoonup }{w}\left(k\right)+e\left(k\right)·\stackrel{\rightharpoonup }{K}\left(k\right)$
   using a small positive number ε.
   $\stackrel{\rightharpoonup }{w}\left(0\right)={\left(\epsilon , \epsilon , ..., \epsilon \right)}^T$

2. Initialize the data vector
   $\stackrel{\rightharpoonup }{\phi }\left(k\right)$
   .
   $\stackrel{\rightharpoonup }{\phi }\left(0\right)={\left(0, 0, ..., 0\right)}^T$

3. Initialize the k × k matrix P(0).
   $P\left(0\right)=\left(\begin{array}{cccc}\epsilon & 0& 0& 0\\ 0& \epsilon & 0& 0\\ 0& 0& ...& 0\\ 0& 0& 0& \epsilon \end{array}\right)$

4. For k = 1, update the data vector
   $\stackrel{\rightharpoonup }{\phi }\left(k\right)$
   based on
   $\stackrel{\rightharpoonup }{\phi }\left(k-1\right)$
   and the current input data u(k) and output data y(k).
5. Compute the predicted response
   $\hat{y}\left(k\right)$
   by solving the following equation:
   $\hat{y}\left(k\right)={\stackrel{\rightharpoonup }{\phi }}^T\left(k\right)·\stackrel{\rightharpoonup }{w}\left(k\right)$

6. Compute the error e(k) by solving the following equation:
   $e\left(k\right)=y\left(k\right)-\hat{y}\left(k\right)$

7. Update the Kalman gain vector
   $\stackrel{\rightharpoonup }{K}\left(k\right)$
   defined by the following equation:
   $\stackrel{\rightharpoonup }{K}\left(k\right)=\frac{P\left(k\right)·\stackrel{\rightharpoonup }{\phi }\left(k\right)}{Q_M+{\stackrel{\rightharpoonup }{\phi }}^T\left(k\right)·P\left(k\right)·\stackrel{\rightharpoonup }{\phi }\left(k\right)}$
   Q_M is the measurement noise and P(k) is a k × k matrix whose initial value is defined by P(0) in step 3.
8. Update the parametric vector
   $\stackrel{\rightharpoonup }{w}\left(k\right)$
   .
   $\stackrel{\rightharpoonup }{w}\left(k+1\right)=\stackrel{\rightharpoonup }{w}\left(k\right)+e\left(k\right)·\stackrel{\rightharpoonup }{K}\left(k\right)$

9. Update the P(k) matrix.
   $P\left(k+1\right)=P\left(k\right)-\stackrel{\rightharpoonup }{K}\left(k\right)·{\stackrel{\rightharpoonup }{\phi }}^T\left(k\right)·P\left(k\right)+Q_P$
   Q_P is the correlation matrix of the process noise.
10. Stop if the error is small enough, else set k = k + 1 and repeat steps 4–10.