世界需要简化第三篇：卡尔曼滤波到底是个什么东西？这一篇让你绝对看懂！——算法经高度抽象简化，所有下标经仔细核对

 

一、用在什么地方？——本博客参考了下列两个博客，将其融合，并高度精简，献给大家，Hope大家都学懂并应用出来。

    根据当前获得的关于对象Object的所有有用信息，得到一个针对对象Object当前状态最好的预测和估计，该预测估计比任何一种有用信息都smarter。

    也许最常见的卡尔曼滤波器是锁相环，它在收音机、计算机和几乎任何视频或通讯设备中广泛存在，你没看错，就是DSP、STM32里的锁相环PLL！

二、卡尔曼滤波的目的？

    针对当前对象Object状态，尽可能在有瑕疵的有用信息中提取最有价值的信息，然后融合这些信息，最终做出状态的最优估计！

    即根据当前状态（k-1 时刻）来预测下一状态（k 时刻）

三、卡尔曼滤波算法的高度简化版：

先给出五大经典公式：（附图是借鉴并致敬，如有侵权请联系编者）

    研究对象是一个房间的温度。假定已知系统状态方程，表示状态变量温度恒定，也就是

     将根据系统模型的估计之外的扰动，看做高斯白噪声，即引入过程噪声Q（如下文的Q=4^2）

    经传感器(温度计)测量，其不确定性看做是高斯白噪声，即引入传感器噪声R（如下文的R=4^2）

    卡尔曼滤波要做什么？根据两个温度值（一个是模型预测值，一个是传感器反馈值），结合Q和R预测估计出最优的实际温度值。

下面开始五大公式应用，快速深入卡尔曼滤波算法应用实战

    由公式1，得k时刻温度假设是23度，计算此时的协方差阵：

得出偏差σ等于5摄氏度，（一般来说，卡尔曼滤波器开始工作只需要随便给一个x估计值和协方差阵P即可，如取x为1，P为100，即偏差为10）：

（假设已知k-1时刻估算出的最优温度估计值的偏差是3摄氏度，即协方差阵Pk-1是3^2，依靠该状态空间表达式模型预测的偏差是4摄氏度（即过程噪声的协方差阵Q等于4^2）。

③计算H阵——需从当前状态x通过左乘H阵，获得与传感器获取数据一致的单位和尺度，因为测量的值来自温度计，与当前状态x(温度值)的单位和尺度直接对应，所以H=1，毋庸置疑

④获得传感器测量值zk=25，和传感器所测值的噪声协方差阵R，即偏差是sqrt(R)：（假设根据温度计测得25度，同时该值的偏差是4度：）

根据两个温度值，如何预测实际温度值？

答案是利用卡尔曼估计理论(Rudolf Emil Kalman)，算法如下：

3.1 第一步——计算卡尔曼系数：

3.2 第二步——根据两个温度值预测实际温度值，该预测值会逐渐收敛：

    即k时刻的实际温度值是： 24.56度。（容易发现，R<Q，所以最优估计结果更加“相信”传感器）

3.3 第三步——计算当前实际预测温度值的偏差（或者是协方差阵）：

3.4 第四步：求得k时刻的最优温度值后，接下来预测k+1时刻进行新的最优温度值估计。

function [] = KermanFilterByMrWang(Realvalue,NumerOfMeasuredPoint,Q,R,guess,Pguess)
%UNTITLED 此处显示有关此函数的摘要a
%   原本系统的状态方程为：
%状态方程为：X(k+1)=1*X(k)+0*u(k)     
%观测方程为：Y(k+1)=1*X(k)
%推出F=1，H=1

%为了模拟观测值，用上述的状态空间表达式，叠加噪声Q，R得到，需给定：
%       X(k+1)=F*X(k)+G*W(k)  
%       Z(k+1)=H*X(k)+V(k)
CON=Realvalue;
N=NumerOfMeasuredPoint;
W=sqrt(Q)*randn(1,N);
V=sqrt(R)*randn(1,N);
F=1;
G=1;
H=1;
I=eye(1); 

Xexpect=CON*ones(1,N);
X=zeros(1,N);  
Xkf=zeros(1,N);
Z=zeros(1,N);
P=zeros(1,N); 


X(1)=guess;%为了生成观测值，也可作为真实温度的波动值

Xkf(1)=guess;%故意取相等于25.1，表示仅由理论状态表达式，获得的值有P的偏差
P(1)=Pguess;%该值就是偏差么
for k=2:N
    X(k)=F*X(k-1)+G*W(k-1);  % 
    Z(k)=H*X(k)+V(k);
    X_pre=F*Xkf(k-1);           
    P_pre=F*P(k-1)*F'+Q;        
    Kg=P_pre*inv(H*P_pre*H'+R); 
    e=Z(k)-H*X_pre;            
    Xkf(k)=X_pre+Kg*e;         
    P(k)=(I-Kg*H)*P_pre;
end

Z(1)=24.9;%为了凑够相同维数。
t=1:N;
figure('Name','Kalman Filter Simulation','NumberTitle','off');
plot(t,Xexpect,'-b',t,X,'-r',t,Z,'-k',t,Xkf,'-g');
legend('expected','real','measure','kalman extimate');         
xlabel('sample time');
ylabel('temperature');
title('Kalman Filter Simulation');
end

附上MATLAB代码，调用时传入参数，以函数形式调用即可（示例如下）：KermanFilterByMrWang(25,120,0.01,0.25,25.1,0.01)；

笔者为了学习卡尔曼滤波，谨参考了以下两篇博客干货的宝贵信息，非常感谢编者老师！

https://blog.csdn.net/Jake_cai/article/details/80376197

https://blog.csdn.net/tiandijun/article/details/72469471

----后续持续更新有关各类算法的简化应用版，希望大家跟我一起简化这个世界！

2021/05/27：北京航空航天大学，Mr Wang