卡尔曼滤波（Kalmon Filter）#附MATLAB代码
卡尔曼滤波介绍
定义：卡尔曼滤波（Kalman Filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。
本质：是利用两个正态分布的融合仍是正态分布这一特性进行迭代。
优点：计算量小，能够利用前一时刻的状态来对当前时刻下的状态进行最优估计，而且只需要保存前一时刻的数据用于迭代即可，不需要大量缓存。
局限性：卡尔曼滤波的局限性在于其只能拟合线性高斯（正态）系统。
卡尔曼滤波5大基本公式
系统状态方程
x k _k k=Ax k − 1 _{k-1} k−1+Bu k − 1 _{k-1} k−1+w k − 1 _{k-1} k−1
状态方程是根据上一时刻的状态和控制变量来推测此刻的状态，w k − 1 _{k-1} k−1是服从高斯分布的噪声，是估计过程的噪声，它对应了 x k _k k 中每个分量的噪声，是期望为 0，协方差为 Q 的高斯白噪声w k − 1 _{k-1} k−1~N(0,Q)。
观测方程
z k _k k= Hx k _k k+v k _k k
v k _k k是观测的噪声，服从高斯分布， v k v_k vk~N(0,R)。
卡尔曼预测方程
X(k|k-1) = AX(k-1|k-1) + Bu(k-1)
P(k|k-1) = AP(k-1|k-1)A T ^T T + Q
卡尔曼更新方程
K k _k k = P(k|k-1)H T ^T T(HP(k|k-1)H T ^T T + R) − 1 ^{-1} −1
X(k|k) = X(k|k-1) + K k _k k(z k _k k - HX(k|k-1))
P(k|k) = (I - K k _k kH)P(k|k-1)
以上，X(k-1|k-1)和X(k|k) 分别表示k-1和k时刻的后验状态估计值，是滤波的结果之一，也叫做最优估计；X(k|k-1) 为k时刻的先验状态估计值，是滤波的中间结果；P(k-1|k-1)和P(k|k)为k-1时刻和k时刻的后验估计协方差，表示状态的不确定度；P(k|k-1)为k时刻的先验估计协方差，是滤波的中间结果。H是状态变量到测量的转换矩阵，将状态和观测量连接起来，在卡尔曼中为线性关系，它负责将m维的的测量值转换成n维；z k _k k为测量（观测）值；K k _k k为滤波增益矩阵，也叫卡尔曼增益或者卡尔曼系数；A为状态转移矩阵，实际上是对目标状态转换的一种猜想模型；Q为过程激励噪声协方差（系统过程的协方差）；R为测量噪声协方差。滤波器实际实现时，测量噪声协方差 R一般可以观测得到，是滤波器的已知条件；B是将输入转换为状态的矩阵；z k _k k - HX(k|k-1)实际观测和预测观测的残差，和卡尔曼增益一起修正先验（预测），得到后验。
卡尔曼滤波实例
小车追踪
《从入门到精通》是我读过最好的解释了，笔者写的很棒，希望下次读的时候会有新的体悟。
卡尔曼滤波MATLAB代码
状态变量为1维，无控制量。房间温度
%%卡尔曼滤波 房间温度模型
N = 200; %采样点个数
temp_expect = 25 * ones(1,N); %期望温度值
temp_real = zeros(1,N); %实际温度值
temp_kalman = zeros(1,N); %卡尔曼滤波值
temp_pre = zeros(1,N); %系统先验值
temp_observe = zeros(1,N); %观测值
P = zeros(1,N); %估计协方差
Q = 0.01; %系统噪声方差
R = 0.25; %观测噪声方差
W=sqrt(Q)*randn(1,N); %系统噪声
V=sqrt(R)*randn(1,N); %观测噪声
temp_real(1) = 25.1;
P(1) = 0.1;
temp_observe(1) = 25;
temp_kalman(1) = temp_observe(1); %可任意取
A = 1; %状态转移矩阵
H = 1; %观测矩阵
for k = 2:N
temp_real(k) = A * temp_real(k-1)+W(k-1);%真实值，实际无法得到
temp_observe(k) = H * temp_real(k)+V(k); %观测值
temp_pre(k) = A * temp_kalman(k-1); %先验估计
P_pre = A*P(k-1)*A'+Q; %先验协方差
K = P_pre * inv((H*P_pre*H'+R)); %卡尔曼增益
temp_kalman(k) = temp_pre(k) + K*(temp_observe(k) - H * temp_pre(k));
P(k) = (1 - K*H) * P_pre; %后验协方差
end
t = 2:N;
plot(t,temp_real(2:N),'-b',t,temp_observe(2:N),'-k',t,temp_kalman(2:N),'-g');
legend('real','observe','kalman extimate');
xlabel('sample time');
ylabel('temperature');
title('Kalman Filter Simulation');
Err_Messure=zeros(1,N);
Err_Kalman=zeros(1,N);
Err_estimate=zeros(1,N);
for k=2:N
Err_Messure(k)=abs(temp_observe(k)-temp_real(k));
Err_Kalman(k)=abs(temp_kalman(k)-temp_real(k));
Err_estimate(k)=abs(temp_pre(k)-temp_real(k));
end
figure('Name','Error Analysis','NumberTitle','off');
plot(t,Err_Messure(2:N),'r-*',t,Err_Kalman(2:N),'b-square',t,Err_estimate(2:N),'g-pentagram')
legend('MessureError','KalmanError','ObserveError');
xlabel('sample time');
ylabel('error');
title('Error Analysis');
结果图
状态变量为2维，有控制量。小车运动
%% 卡尔曼滤波 小车运动
N = 30; %采样点个数
t = 1:N; %1s的间隔
x_real = zeros(2,N); %实际温度值
x_kalman = zeros(2,N); %卡尔曼滤波值
x_pre = zeros(2,N); %系统先验值
x_observe = zeros(1,N); %观测值
P = zeros(2,2,N); %估计协方差
Q = [0.5,0;0,0.5]; %系统噪声方差
R = 2; %观测噪声方差
W=sqrt(Q)*randn(2,N); %系统噪声
V=sqrt(R)*randn(1,N); %观测噪声
x_real(:,1) = [0;0]; %初始位置速度都为0
P(:,:,1) = [1,0;0,1]; %后验估计协方差
x_observe(1) = 0;
x_kalman(:,1) = [10;3]; %可任意取
A = [1,1;0,1]; %状态转移矩阵
H = [1,0]; %观测矩阵
B = [0.5;1]; %输入量矩阵
a = 1; %加速度
for k = 2:N
x_real(:,k) = A * x_real(:,k-1)+a*B+W(:,k-1);%真实值，实际无法得到
x_observe(k) = H * x_real(:,k)+V(k); %观测值
x_pre(:,k) = A * x_kalman(:,k-1)+a*B; %先验估计
P_pre = A*P(:,:,k-1)*A'+Q; %先验协方差
K = P_pre *H'* inv((H*P_pre*H'+R)); %卡尔曼增益
x_kalman(:,k) = x_pre(:,k) + K*(x_observe(k) - H * x_pre(:,k));
P(:,:,k) = ([1,0;0,1] - K*H) * P_pre; %后验协方差
end
plot(t,x_real(1,:),'-*',t,x_kalman(1,:),'-o',t,x_observe,'-^')
legend('real','kalman extimate','observe');
figure
plot(t,x_real(2,:),'-*',t,x_kalman(2,:),'-o',t,x_pre(2,:),'-^')
legend('real','kalman extimate','pre');
结果图
参考资料
卡尔曼滤波器（百度百科）
卡尔曼五大公式
卡尔曼滤波案例
卡尔曼从入门到精通