卡尔曼滤波器以Rudolf Kalman命名，是一种优化估算算法，用于存在测量误差情况下预测参数，广泛用于控制科学，计算机视觉，信号处理等领域。
注意：在了解卡尔曼滤波器之前需要先知道状态观测器

状态观测器

航天器从地球前往火星，需要时刻监测反应室的温度，如果温度过高不采取措施会导致其他设备出现故障，然而不能直接把温度传感器放置在反应室内，必须放在反应室外部的低温表面测量温度；

根据热传导的理论，可以联想到以下数学模型，输入量是燃油流量WfuelW_{fuel}Wfuel，中间状态是反应室温度TinT_{in}Tin，可观测的量是反应室外部的温度TextT_{ext}Text：

记变量x^\widehat{x}x表示对变量xxx的估计，则对于以上的实际系统，输入的量相同，当实际测量的输出量与热传导数学模型计算的输出很接近时，可以认为这个无法测量的TinT_{in}Tin与模型计算得到T^in\widehat{T}_{in}Tin也是相接近的：

但现在的问题是如何让这个模型计算的输出量T^ext\widehat{T}_{ext}Text越来越收敛于实际测量的TextT_{ext}Text，这时就需要用一个闭环的反馈来实现，这就构成了状态观测器（蓝色部分）：

现在面临的问题就变成确定控制器K的参数，使T^ext\widehat{T}_{ext}Text快速收敛于TextT_{ext}Text；

值得一提，现在以现代控制理论的方式理解这个系统：

状态求差并化简后有：
e˙obs=(A−KC)eobs\dot{e}_{obs}=(A-KC)e_{obs}e˙obs=(A−KC)eobs
反拉普拉斯变换可以得到状态方程的解：
eobs(t)=e(A−KC)teobs(0)e_{obs}(t)=e^{(A-KC)t}e_{obs}(0)eobs(t)=e(A−KC)teobs(0)
可以看出，如果(A−KC)<0(A-KC)<0(A−KC)<0，则误差最终将收敛至0：

卡尔曼滤波算法的原理

为了简单分析，重新考虑一个在一维上的驾驶系统，输入为当前时刻kkk的速度uku_{k}uk，输出为所在位置yky_{k}yk，并令输出等于当前的状态即yk=Cxk,C=1y_{k}=Cx_{k},C=1yk=Cxk,C=1；
如果考虑坏境因素，测量位置时加入测量误差vkv_{k}vk；驾驶中，状态会受到风速于路况影响即加入过程误差wkw_{k}wk，将状态空间离散化表示为：

根据统计，测量误差与过程误差一般都服从正态分布；
假设本问题中：
w∼N(0,Q),v∼N(0,R)w\sim N(0,Q),v\sim N(0,R)w∼N(0,Q),v∼N(0,R)

现在单纯想构造状态观测器，观测到状态(驾驶系统所在位置)：

可以看出，在现实的系统中，存在着不确定性的误差，而驾驶模型本身不会考虑误差，这就导致估计状态x^k\widehat{x}_{k}xk由于误差的积累而难以收敛到测量的状态xkx_{k}xk，此时就需要卡尔曼滤波算法，融合两者信息得到一个新的最优估计；
实际上卡尔曼滤波器本身就是一个状态观测器，只不过它是专为随机系统设计的。常规的状态观测器用于确定性的系统：

对于使用卡尔曼滤波器得到的估计状态：

第一部分：
Ax^k−1+BukA\widehat{x}_{k-1}+Bu_{k}Axk−1+Buk
被称为先验估计，记为x^k−\widehat{x}_{k}^{-}xk−，其中x^k−1\widehat{x}_{k-1}xk−1来自数学模型的估计；
第二部分：
Kk(yk−Cx^k−)K_{k}(y_{k}-C\widehat{x}_{k}^{-})Kk(yk−Cxk−)
其中yky_{k}yk来自于测量的输出量，第一部分加第二部分计算后的结果x^k\widehat{x}_{k}xk被称为后验估计；

卡尔曼滤波算法过程

上面式子中，还有KkK_{k}Kk要计算，算法过程分两步：
首先，数学模型用于计算状态的先验估计：
x^k−=Ax^k−1+Buk\widehat{x}_{k}^{-}=A\widehat{x}_{k-1}+Bu_{k}xk−=Axk−1+Buk
Pk−=APk−1AT+QP_{k}^{-}=AP_{k-1}A^{T}+QPk−=APk−1AT+Q
下一步，更新：
Kk=Pk−CTCPk−CT+RK_{k}=\frac{P_{k}^{-}C^{T}}{CP_{k}^{-}C^{T}+R}Kk=CPk−CT+RPk−CT
x^k=x^k−+Kk(yk−Cx^k−)\widehat{x}_{k}=\widehat{x}_{k}^{-}+K_{k}(y_{k}-C\widehat{x}_{k}^{-})xk=xk−+Kk(yk−Cxk−)
Pk=(I−KkC)Pk−P_{k}=(I-K_{k}C)P_{k}^{-}Pk=(I−KkC)Pk−

注意，P,Q,RP,Q,RP,Q,R分别为状态协方差矩阵，过程误差协方差矩阵，测量噪声协方差矩阵；

方差，协方差，协方差矩阵

对于随机变量XXX，采样到nnn个样本，X‾\overline{X}X为样本的均值(期望是对于概率而言的)，其方差为：
var(X)=∑i=1n(Xi−X‾)(Xi−X‾)n−1var(X)=\frac{\sum_{i=1}^{n}(X_{i}-\overline{X})(X_{i}-\overline{X})}{n-1}var(X)=n−1∑i=1n(Xi−X)(Xi−X)
而对于两个不同的随机变量XXX和YYY，则协方差为：
Cov(X,Y)=∑i=1n(Xi−X‾)(Yi−Y‾)n−1Cov(X,Y)=\frac{\sum_{i=1}^{n}(X_{i}-\overline{X})(Y_{i}-\overline{Y})}{n-1}Cov(X,Y)=n−1∑i=1n(Xi−X)(Yi−Y)
一般地，对于随机变量组成的向量X={X1,X2,...,XN}X=\left \{ X_{1},X_{2},...,X_{N} \right \}X={X1,X2,...,XN}，称矩阵CCC为协方差矩阵：

其中，ci,j=Cov(Xi,Xj)c_{i,j}=Cov(X_{i},X_{j})ci,j=Cov(Xi,Xj)；
注意：

均值反映了样本分布的平均；
方差描述样本分布的离散程度，也就是该变量对于其期望的距离；
协方差表示的是两个变量的总体的误差，这与只表示一个变量误差的方差不同。如果两个变量的变化趋势一致，也就是说如果其中一个大于自身的期望值，另外一个也大于自身的期望值，那么两个变量之间的协方差就是正值。如果两个变量的变化趋势相反，即其中一个大于自身的期望值，另外一个却小于自身的期望值，那么两个变量之间的协方差就是负值；
协方差矩阵的每个元素是各个向量元素之间的协方差，是从标量随机变量到高维度随机向量的自然推广；

现在可以知道卡尔曼滤波算法中的P,Q,RP,Q,RP,Q,R如何计算：
1.对于状态协方差矩阵PPP：状态协方差矩阵PPP就是各个状态之间的协方差组成的矩阵；
2.对于过程误差协方差矩阵QQQ：该矩阵的每一个元素分别是各个状态的过程误差之间的协方差，各个状态的误差不容易确定，对于具体问题有具体的定义；比如对于车辆行驶，假设状态方程中有位置与速度两个状态，状态的误差可能需要考虑风力与地面摩擦力，力影响着加速度，所以要将误差从加速度推导到速度再推导到位移，最后根据两状态间的过程误差，计算协方差矩阵；
3.对于测量噪声协方差矩阵RRR：来源是传感器对于可测量变量的误差，在使用时一般传感器都会给出精度指标，该指标就可以直接转化到矩阵RRR中；

基于Matlab的卡尔曼滤波器实例

假设现在要测量一个静止区域的温度，数学模型也很简单：房间当前温度真实值为24度，认为下一时刻与当前时刻温度相同；
没有输入量即u=0u=0u=0，令输出量温度即为状态，状态只有一个，由此可建立模型：
xk=xk−1x_{k}=x_{k-1}xk=xk−1
yk=xky_{k}=x_{k}yk=xk
在实际系统中，存在由于外界环境导致的过程误差QQQ和温度计的测量误差RRR，两个误差均服从正态分布，现在要用卡尔曼滤波器对温度计的测量值进行优化估计，模拟如下：

clear all;
% 房间当前温度真实值为24度，认为下一时刻与当前时刻温度相同，误差为0.02度，即认为连续的两个时刻最多变化0.02度。
% 温度计的测量误差为0.5度。
% 开始时，房间温度的估计为23.5度，误差为1度。
close all;% intial parameters
% 计算连续n_iter个时刻
n_iter = 100;
% size of array. n_iter行，1列
sz = [n_iter, 1];
% 温度的真实值
x = 24;
% 过程方差，反应连续两个时刻温度方差
Q = 0.01;
% 测量方差，反应温度计的测量精度
R = 0.25;
% z是温度计的测量结果，在真实值的基础上加上了方差为0.25的高斯噪声。
z = x + sqrt(R)*randn(sz);
% 对数组进行初始化
% 对温度的后验估计。即在k时刻，结合温度计当前测量值与k-1时刻先验估计，得到的最终估计值
xhat = zeros(sz);
% 后验估计的方差
P = zeros(sz);
% 温度的先验估计。即在k-1时刻，对k时刻温度做出的估计
xhatminus = zeros(sz);
% 先验估计的方差
Pminus = zeros(sz);
% 卡尔曼增益，反应了温度计测量结果与过程模型（即当前时刻与下一时刻温度相同这一模型）的可信程度
K = zeros(sz);
% intial guesses
xhat(1) = 23.5; %温度初始估计值为23.5度
P(1) = 1; % 误差为1for k = 2:n_iter% 时间更新（预测）% 用上一时刻的最优估计值来作为对当前时刻的温度的预测xhatminus(k) = xhat(k-1);% 预测的方差为上一时刻温度最优估计值的方差与过程方差之和Pminus(k) = P(k-1)+Q;% 测量更新（校正）% 计算卡尔曼增益K(k) = Pminus(k)/( Pminus(k)+R );% 结合当前时刻温度计的测量值，对上一时刻的预测进行校正，得到校正后的最优估计。该估计具有最小均方差xhat(k) = xhatminus(k)+K(k)*(z(k)-xhatminus(k));% 计算最终估计值的方差P(k) = (1-K(k))*Pminus(k);
endFontSize = 14;
LineWidth = 1;
figure();
plot(z,'ro-','LineWidth',LineWidth); %画出温度计的测量值
hold on;
plot(xhat,'bo-','LineWidth',LineWidth) %画出最优估计值
hold on;
plot(x*ones(sz),'g-','LineWidth',LineWidth); %画出真实值
legend('温度计的测量结果', '后验估计', '真实值');
xl = xlabel('时间(分钟)');
yl = ylabel('温度');

其他算法-卡尔曼滤波器相关推荐

扩展卡尔曼滤波器（EKF）
线性卡尔曼滤波的算法采用递推的形式表示,其算法已被学者们研究得较为透彻.但是实际应用中非线性的现象是十分普遍的,例如:物理模型受到外部干扰.存在非线性性和病态方差矩阵等因素所引起的非线性问题都使得传统 ...
基于MATLAB的RSSI 和 PLE 定位算法，并通过卡尔曼滤波器减少非视距误差
基于MATLAB的RSSI 和 PLE 定位算法,并通过卡尔曼滤波器减少非视距误差根据上面的课题要求,我们知道在室内未知信道环境下,进行RSS估计效果较差,而本课题所要求的是在室内未知PL的情况下进 ...
卡尔曼滤波器求速度matlab,卡尔曼滤波器算法浅析及matlab实战
原标题:卡尔曼滤波器算法浅析及matlab实战作者:Liu_LongPo 出处:Liu_LongPo的博客卡尔曼滤波器是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的 ...
车流量检测实现：多目标追踪、卡尔曼滤波器、匈牙利算法、SORT/DeepSORT、yoloV3、虚拟线圈法、交并比IOU计算
日萌社人工智能AI:Keras PyTorch MXNet TensorFlow PaddlePaddle 深度学习实战(不定时更新) CNN:RCNN.SPPNet.Fast RCNN.Faste ...
匈牙利算法和卡尔曼滤波器
1.匈牙利算法 **分配问题(Assignment Problem):**假设有N个人和N个任务,每个任务可以任意分配给不同的人,已知每个人完成每个任务要花费的代价不尽相同,那么如何分配可以使得总的代 ...
【数学和算法】初识卡尔曼滤波器（六）
本文是观看B站视频[卡尔曼滤波器]6_扩展卡尔曼滤波器_Extended Kalman Filter所做的截图和笔记. 前面讲的都是线性系统,他们可以用卡尔曼滤波器进行预测. 对于非线性系统,可以将非 ...
【数学和算法】初识卡尔曼滤波器（四）
本文是观看B站视频教程[卡尔曼滤波器]4_误差协方差矩阵数学推导_卡尔曼滤波器的五个公式所做的截图和笔记. 先列出前面几篇博客得到的公式: 下面是利用上图公式推导Pk−\displaystyle\c ...
【数学和算法】初识卡尔曼滤波器（三）
本文是观看B站视频教程所做的截图和笔记. 疑问: 1.观测矩阵H是单位矩阵吗?如果是的话,那么卡尔曼增益的两种写法都合理.那么他的作用是对XkX_kXk向量进行变换为ZkZ_kZk向量的各个维度对 ...
【数学和算法】初识卡尔曼滤波器（二）
1.数据融合用两个工具测量某物体重量的结果分别为30g,32g,这两种工具的测量标准差分别为2g,4g,请估计该物体的真实值. 最终得到该物体的估计值为30.4g,标准差为1.79. 以上就是数据融 ...

其他算法-卡尔曼滤波器

目录