卡尔曼滤波（Kalman filter）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。
卡尔曼滤波器的原理解释如下：
首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程（Linear Stochastic Difference equation）来描述：
X(k)=A X(k-1)+B U(k)+W(k)
再加上系统的测量值：
Z(k)=H X(k)+V(k)
上两式子中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，他们为矩阵。Z(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，他们的covariance 分别是Q，R（这里我们假设他们不随系统状态变化而变化）。
对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。下面我们来用他们结合他们的covariances 来估算系统的最优化输出（类似上一节那个温度的例子）。
首先我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是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表示covariance：
P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)
推导过程：
e(k|k-1)=X(k)-X(k|k-1)=A( x(k)-x(k-1|k-1) ) + W(k)
P(k|k-1)=E{ ( X(k) - X(k|k-1) ) ( X(k) - X(k|k-1) )’}
=E{ ( AX(k-1)+BU(k)+W(k)-AX(k-1|k-1)-BU(k) ) ( AX(k-1) + BU(k) + W(k) - AX(k-1|k-1) - BU(k) )’}
=E{ ( A( X(k-1) - X(k-1|k-1) ) + W(k) ) ( A( X(k-1) - X(k-1|k-1) )+W(k) )’}
=E{ A( X(k-1) - X(k-1|k-1) ) ( X(k-1)-X(k-1|k-1) )’A’+ W(k)W(k)’}
=A E{ ( X(k-1) - X(k-1|k-1) ) ( X(k-1) - X(k-1|k-1) )’} A’+ E{ W(k)W(k)’}
=A P(k-1|k-1) A’+ Q
备注：因W与X及其估计值无关，所以相乘项略去。
式(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)的最优化估算值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)。这样，算法就可以自回归的运算下去。
公式(4)(5)推导：
令e(k|k)=X(k)-X(k|k)
代入(1)(3)式和Z(k)的表达式，得e(k)=( I - Kg(k)H )e(k|k-1)-Kg(k)V(k)
P(k|k)=E{ e(k|k) e(k|k)’}
=( I-Kg(k)H ) P(k|k-1) (I-Kg(k)H)’+Kg(k)RKg(k)’
P(k|k)对Kg(k)求微分使得并使求微分后的方程等于0，即此时的Kg(k)可使P(k|k)=0，得：
Kg(k)=P(k|k-1) H’/ (H P(k|k-1) H’+ R)
从而也得到(5)式。
卡尔曼滤波器的原理基本描述了，式子1，2，3，4和5就是他的5 个基本公式。根据这5个公式，可以很容易的实现计算机的程序。
*以上内容部分节选自http://blog.chinaunix.net/uid-26694208-id-3184442.html
一个小例子
假设我们有能力测量电源的电压值，但是这个测量被0.1伏RMS白噪声（例如我们的模数转换器不是很准确）损坏。 下面是真实电压和测量电压的图表。
并有如下假设
A = 1; % the state does not change
B = 0; % there is no control input
H = 1; % our voltage measurement, zk, is of the state directly
Q = 0.00001; % let’s assume a small process variance
R = 0.01;
x(1) = 0;
p(1) = 1; % if we’re certain about xhat(1)=0 then P(1)=0
其中，x初始值不用太在意，随便给一个就可以了，因为随着卡尔曼的工作，x会逐渐的收敛。但是对于p，一般不要取0，因为这样可能会令卡尔曼完全相信你给定的x(0|0)是系统最优的，从而使算法不能收敛。
由此我们可以简化上一节中的五个方程。
X(k|k-1)=X(k-1|k-1) ……….. (6)
P(k|k-1)=P(k-1|k-1) +Q ……… (7)
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1)) ……… (8)
Kg(k)= P(k|k-1) / (P(k|k-1) + R) ……… (9)
P(k|k)=（1-Kg(k)）P(k|k-1) ……… (10)
以下是此例子的卡尔曼滤波matlab程序:
%-------------------------------------------------------------------------
% DESC: Simulation of measured voltage measurements with 0.1 volt RMS white
% noise (actual voltage should be -0.37727 volts).
% The Kalman filter is used to estimate the optimal voltage value.
%-------------------------------------------------------------------------
clear all; clc;
N=50;
x(1)=0; %初始估计状态
p(1)=1; %初始估计协方差
Q=10^-6; %过程噪声的协方差
R=10^-2; %观测噪声的协方差
t=1:N;
%---------------------NOISY MEASUREMENT-----------------------------------
voltage = [-0.46687,-0.36375,-0.39117,-0.49361,-0.2589,-0.37881,-0.32365,...
-0.44891,-0.44283,-0.34583,-0.36659,-0.19245,-0.40478,-0.15601,-0.22642,...
-0.57178,-0.54532,-0.43462,-0.39585,-0.37638,-0.29358,-0.4495,-0.44942,...
-0.39739,-0.37932,-0.34938,-0.27144,-0.3151,-0.55233,-0.30754,-0.29612,...
-0.31364,-0.24626,-0.34456,-0.44457,-0.3922,-0.62217,-0.32994,-0.36558,...
-0.43638,-0.44274,-0.48534,-0.38204,-0.33934,-0.41031,-0.42726,-0.38087,...
-0.39475,-0.473,-0.24802;];
%-------------------------------------------------------------------------
for i=2:N
y(i)=x(i-1); %预测的状态
p1(i)=p(i-1)+Q; %预测的协方差
kg(i)=p1(i)/(p1(i)+R); %卡尔曼增益
x(i)=y(i)+kg(i)*(voltage(i)-y(i)); %更新的状态
p(i)=(1-kg(i))*p1(i); %更新的协方差
end
plot(t,x,'r');
hold on
plot([0 50],[-0.37727 -0.37727],'k');
plot(voltage,'-b');
axis([0 50 -0.7 0]);
xlabel('time');
ylabel('voltage');优化结果如下图所示
