在學習卡爾曼濾波器之前，首先看看為什麼叫“卡爾曼”。跟其他著名的理論（例如傅立葉變換，泰勒級數等等）一樣，卡爾曼也是一個人的名字，而跟他們不同的是，他是個現代人！

卡爾曼全名Rudolf Emil Kalman，匈牙利數學家，1930年出生於匈牙利首都布達佩斯。1953，1954年於麻省理工學院分別獲得電機工程學士及碩士學位。1957年於哥倫比亞大學獲得博士學位。我們現在要學習的卡爾曼濾波器，正是源於他的博士論文和1960年發表的論文《A New Approach to Linear Filtering and Prediction Problems》（線性濾波與預測問題的新方法）。如果對這編論文有興趣，可以到這裡的地址下載： 

1、卡爾曼濾波

（Introduction to the Kalman Filter）

為了可以更加容易的理解卡爾曼濾波器，這裡會應用形象的描述方法來講解，而不是像大多數參考書那樣羅列一大堆的數學公式和數學符號。但是，他的5條公式是其核心內容。結合現代的計算機，其實卡爾曼的程式相當的簡單，只要你理解了他的那5條公式。

2、卡爾曼濾波演算法

卡爾曼濾波有兩組基本的方程，狀態運動方程和觀測方程，這是需要預先給定的。

X(k)=A(k) X(k-1)+B U(k)+W(k)
再加上系統的測量值：
Z(k)=H (k)X(k)+V(k)
上兩式子中，X(k)是k時刻的系統狀態，U(k)是k時刻對系統的控制量。A稱為狀態轉移矩陣。可以看出狀態運動方程反映了k-1時刻目標的狀態與k時刻目標狀態之間的關係，所以我們稱之為狀態運動方程。
Z(k)是k時刻的觀測值，H是觀測矩陣，表示人們對於k時刻狀態進行觀測時，獲取觀測量的機制。W(k)和V(k)分別表示狀態方程和觀測方程的噪聲。這個方程是描述觀測第k時刻目標狀態的機理。在實際問題中，通常假設A(k)=A,H(k) = H.因此我們可以看出，在應用卡爾曼濾波時，首先必須要給出運動方程和觀測方程，也就是說，要對目標運動及觀測過程進行建模，並且模型是線性的，稱之為"Model-based"。這是卡爾曼濾波最大的一個缺陷。
假設W和V為高斯白噪聲(White Gaussian Noise)，他們的covariance 分別是Q，R（這裡我們假設他們不隨系統狀態變化而變化）。
首先我們要利用狀態方程預測下一狀態的系統。假設當前時刻為k，根據系統的模型，可以基於系統的上一狀態而預測出現在狀態：
X(k|k-1)=A X(k-1|k-1)+B U(k) ………..                                 (1)
式(1)中，X(k|k-1)是利用上一狀態預測的結果，X(k-1|k-1)是上一狀態最優的結果，U(k)為現在狀態的控制量，如果沒有控制量，它可以為0。因為是利用狀態方程對狀態進行更新，反映了狀態隨時間演化的規律，因此，稱之為時間更新。
到現在為止，我們的系統結果已經更新了，可是，對應於X(k|k-1)的covariance，即上面預測值相對於真實狀態的誤差方差陣更新公式：
P(k|k-1)=A P(k-1|k-1) A’+Q ………                                       (2)
式(2)中，P(k|k-1)是X(k|k-1)對應的covariance，P(k-1|k-1)是X(k-1|k-1)對應的covariance，A’表示A的轉置矩陣，Q是系統過程的covariance。式子1，2就是卡爾曼濾波器5個公式當中的前兩個，也就是對系統的預測。
當第k時刻的觀測值Z(k)到達以後，我們希望利用它去修正第k個時刻的狀態預測值。怎麼去修正呢？當然是要用預測值中沒有的資訊去修正預測值了。因此Z(k)-H X(k|k-1)就是新的資訊。結合預測值和測量值，我們可以得到現在狀態(k)的最優化估算值X(k|k)：
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ………                  (3)
其中Kg為卡爾曼增益(Kalman Gain)：
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ………                    (4)
到現在為止，我們已經得到了k狀態下最優的估算值X(k|k)。但是為了要令卡爾曼濾波器不斷的執行下去直到系統過程結束，我們還要更新k狀態下X(k|k)的covariance：
P(k|k)=（I-Kg(k) H）P(k|k-1) ………                                    (5)
其中I 為1的矩陣，對於單模型單測量，I=1。當系統進入k+1狀態時，P(k|k)就是式子(2)的P(k-1|k-1)。這樣，演算法就可以自迴歸的運算下去。
卡爾曼濾波器的原理基本描述了，式子1，2，3，4和5就是他的5 個基本公式。根據這5個公式，可以很容易用計算機程式設計實現。
假設一個接近勻速運動的小球，初始位置為24，既然是“接近勻速”，必有誤差，誤差為0.04，狀態轉移矩陣是常數1。如果有一個儀器對位移進行測量，獲得的是位移值，則觀測矩陣也是1，測量一定有誤差，其誤差為0.25.從下面的圖可以看出估計值即濾波與其真實值很接近，但是量測值與真實值相去甚遠。
--------------------- 
作者：Particlefilter 
來源：CSDN 
原文：https://blog.csdn.net/u010545732/article/details/18989051 
版權宣告：本文為博主原創文章，轉載請附上博文連結！

3、卡爾曼濾波工具箱

原文地址：http://www.cs.ubc.ca/~murphyk/Software/Kalman/kalman.html

Written by Kevin Murphy, 1998.

Last updated: 7 June 2004.

This toolbox supports filtering, smoothing and parameter estimation (using EM) for Linear Dynamical Systems.

• Download toolbox
• What is a Kalman filter?
• Example of Kalman filtering and smoothing for tracking
• What about non-linear and non-Gaussian systems?
• Other software for Kalman filtering, etc.
• Recommended reading

Functions

• kalman_filter
• kalman_smoother - implements the RTS equations
• learn_kalman - finds maximum likelihood estimates of the parameters using EM
• sample_lds - generate random samples
• AR_to_SS - convert Auto Regressive model of order k to State Space form
• SS_to_AR
• learn_AR - finds maximum likelihood estimates of the parameters using least squares

What is a Kalman filter?

For an excellent web site, see Welch/Bishop's KF page. For a brief intro, read on...

A Linear Dynamical System is a partially observed stochastic process with linear dynamics and linear observations, both subject to Gaussian noise. It can be defined as follows, where X(t) is the hidden state at time t, and Y(t) is the observation.

   x(t+1) = F*x(t) + w(t),  w ~ N(0, Q),  x(0) ~ N(X(0), V(0))
   y(t)   = H*x(t) + v(t),  v ~ N(0, R)

The Kalman filter is an algorithm for performing filtering on this model, i.e., computing P(X(t) | Y(1), ..., Y(t)).
The Rauch-Tung-Striebel (RTS) algorithm performs fixed-interval offline smoothing, i.e., computing P(X(t) | Y(1), ..., Y(T)), for t <= T.

Example of Kalman filtering

Here is a simple example. Consider a particle moving in the plane at constant velocity subject to random perturbations in its trajectory. The new position (x1, x2) is the old position plus the velocity (dx1, dx2) plus noise w.

[ x1(t)  ] =  [1 0 1 0] [ x1(t-1)  ] + [ wx1  ]
[ x2(t)  ]    [0 1 0 1] [ x2(t-1)  ]   [ wx2  ]
[ dx1(t) ]    [0 0 1 0] [ dx1(t-1) ]   [ wdx1 ]
[ dx2(t) ]    [0 0 0 1] [ dx2(t-1) ]   [ wdx2 ]

We assume we only observe the position of the particle.

[ y1(t) ] =  [1 0 0 0] [ x1(t)  ] + [ vx1 ]
[ y2(t) ]    [0 1 0 0] [ x2(t)  ]   [ vx2 ]
                       [ dx1(t) ] 
                       [ dx2(t) ]

Suppose we start out at position (10,10) moving to the right with velocity (1,0). We sampled a random trajectory of length 15. Below we show the filtered and smoothed trajectories.

The mean squared error of the filtered estimate is 4.9; for the smoothed estimate it is 3.2. Not only is the smoothed estimate better, but we know that it is better, as illustrated by the smaller uncertainty ellipses; this can help in e.g., data association problems. Note how the smoothed ellipses are larger at the ends, because these points have seen less data. Also, note how rapidly the filtered ellipses reach their steady-state (Ricatti) values. (Click here to see the code used to generate this picture, which illustrates how easy it is to use the toolkit.)

What about non-linear and non-Gaussian systems?

For non-linear systems, I highly recommend the ReBEL Matlab package, which implements the extended Kalman filter, the unscented Kalman filter, etc. (See Unscented filtering and nonlinear estimation, S Julier and J Uhlmann, Proc. IEEE, 92(3), 401-422, 2004. Also, a small correction.)

For systems with non-Gaussian noise, I recommend Particle filtering (PF), which is a popular sequential Monte Carlo technique. See also this discussion on pros/cons of particle filters. and the following tutorial: M. Arulampalam, S. Maskell, N. Gordon, T. Clapp, "A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking," IEEE Transactions on Signal Processing, Volume 50, Number 2, February 2002, pp 174-189 (pdf cached here The EKF can be used as a proposal distribution for a PF. This method is better than either one alone. The Unscented Particle Filter, by R van der Merwe, A Doucet, JFG de Freitas and E Wan, May 2000.Matlab software for the UPF is also available.

Gatsby reading group on nonlinear dynamical systems

Other packages for Kalman filtering and state-space models

SSPIR package in R. The system identification toolbox from the Mathworks implements many classical algorithms.ARfit is an excellent package for autoregressive models.Zoubin Ghahramani has matlab code for EM in LDS's which is similar to mine, but is partially written in C.KBF, an implementation of the Kalman filter-smoother in Omatrix, a (supposedly faster) version of matlab.Le Sage's econometrics toolbox, contains lots of excellent matlab time series modelling functionsEconometric Links Econometrics Journal. Most of the software is either commercial or written in Gauss, which is similar to Matlab.SSF pack is a set of C routines for state-space filtering.Stamp is a commercial package for structural time series analysis. Statistical Time Series Analysis Toolbox O matrix Statistical Time Series Analysis Toolbox

Recommended reading

Welch & Bishop, Kalman filter web page, the best place to start.T. Minka, "From HMMs to LDSs", tech report.T. Minka, Bayesian inference in dynamic models -- an overview , tech report, 2002K. Murphy. "Filtering, Smoothing, and the Junction Tree Algorithm",. tech. report, 1998.Roweis, S. and Ghahramani, Z. (1999) A Unifying Review of Linear Gaussian Models Neural Computation 11(2):305--345.M. Arulampalam, S. Maskell, N. Gordon, T. Clapp, "A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking," IEEE Transactions on Signal Processing, Volume 50, Number 2, February 2002, pp 174-189.A. Doucet, N. de Freitas and N.J. Gordon, "Sequential Monte Carlo Methods in Practice", Springer-Verlag, 2000

4、卡爾曼濾波工具箱另一版本

http://becs.aalto.fi/en/research/bayes/ekfukf/install.html