一、简单概括
一句话概括：
已知一个测量值及其误差分布，一个预估值及其误差分布，推测一个更准确的值及其误差分布。
正经话：
卡尔曼滤波是一种最优估计，本质上就是从有噪声的测量信号中过滤出对我们最有用的信号。
它的特点在于它并不是只通过当前的测量信号来预估信号，因为测量信号也可能是不准确的，它是有一定误差范围的，因此卡尔曼滤波还结合了被测系统自身的性质（数学建模），给出了一个预估值，最后通过结合测量值和预估值得到最优结果。
因为测量值和预估值都是有一定误差范围的，并且误差是符合某种分布的，因此卡尔曼滤波的精髓（那些麻烦的方程）就是如何根据两者的误差分布来得到一个新的更加准确的误差分布。 或者说测量值和预估值都有一定的可信性，但是可信程度不一样，两者都得参考，但是如何定量的去分配相信它们的程度，这就是卡尔曼滤波做的事。
二、举例说明
例子1：温度版本
在房间里，某时刻用温度计测得温度15°C，但是这个温度计很不准，其范围是±4°C。一个小时后又测得温度20°C，同样其误差范围是±4°C，然而我们根据屋内外的气候条件进行物理建模，推测出一个小时后的温度应该为18°C，推测误差为±3°C。现在我们有两个值（一个温度计测出来的，一个我们建模推算出来的），和对应的两个误差范围，那么如何根据它们得到一个新的更准确的值，以及新值的误差范围，这就是卡尔曼滤波干的事。
例子2：小车版本
一辆小车在高速路上狂奔，在某时刻GPS给它测得一个定位坐标，比如在距离某点100m处，同样，这个坐标是有误差范围的，假设为±5m。十秒钟后，GPS再次测得小车到了200m处，误差同样为±5m。这时候我们想更准确的确定小车的位置，怎么办？我们还有小车自带的惯性导航系统，再利用我们的数学知识：位置、速度、加速度那一套对小车进行数学建模分析，我们得到一个预估值195m，误差为±1m。现在，与例子一相同，我们得到了一个测量值及其误差范围，一个预估值及其误差范围，那么如果根据这些来得到一个更准确的值及其误差范围，这就是卡尔曼滤波要解决的了。
见下面的直观图：
（图片来源：https://www.zhihu.com/question/23971601，讲的真的不错，比那些死板的教材好太多了，这是matlab出的导学视频，然后一个好心人做了字幕）
如上图所示，右边的Predicted state estimate（预估状态，蓝色）曲线就是我们的预估值及其误差范围（原始的卡尔曼滤波是假设了误差是符合高斯分布的），橙色的Measurement线就代表了测量值及其误差分布。最后，中间的灰色部分，就是由两条线（预估值及其误差分布，测量值及其误差分布）得到了更加准确的值以及误差分布。
三、公式解释（线性离散系统）
那么卡尔曼滤波是如何根据一个测量值及其误差范围，一个预估值及其误差范围来得到一个更准确的值及其误差范围的呢？这就要从公式上来看了。
A、系统的建模方程
如果你对卡尔曼滤波有个大致了解，那么应该清楚卡尔曼滤波有着非常经典的五个公式！那么在给出卡尔曼滤波经典的五个方程之前，先给出这五个方程基于的系统模型，因为前面的两个例子只是简单的说明，真正在使用的过程中，还是需要对我们的问题进行一个系统建模的。
{
X
k
=
Φ
k
,
k
−
1
X
k
−
1
+
Γ
k
−
1
W
k
−
1
Z
k
=
H
k
X
k
+
V
k
\left\{\begin{aligned}&\boldsymbol{X}_{k}=\boldsymbol{\Phi}_{k, k-1} \boldsymbol{X}_{k-1}+\boldsymbol{\Gamma}_{k-1} \boldsymbol{W}_{k-1}\\&\boldsymbol{Z}_{k}=\boldsymbol{H}_{k} \boldsymbol{X}_{k}+\boldsymbol{V}_{k}\end{aligned}\right.
{Xk=Φk,k−1Xk−1+Γk−1Wk−1Zk=HkXk+Vk
- 首先k代表当前时刻，k-1代表上一时刻，比如我们前面的两个例子中k时刻的预估值都是根据上一时刻的值得到的。
- X \boldsymbol{X} X为一个n维的状态变量，可以理解成我们要知道的一些变量，比如速度、加速度等拼凑起来的一个n维的未知变量组合。 Φ k , k − 1 \boldsymbol{\Phi}_{k, k-1} Φk,k−1表示k-1时刻到k时刻的一步转移矩阵，意思就是说上一时刻的状态变量得乘以这个矩阵后才能得到当前时刻的状态变量。
- Z \boldsymbol{Z} Z是我们的量测量。既然上面说状态量是我们想要知道的一些变量，那量测量也是我们想要知道的变量啊，为什么它和状态变量还要分开呢？因为量测量往往是指我们的装置能够直接得到的，而部分状态变量往往是间接得到的，比如我们的测量装置只能观察到不同时刻物体的位置，这个位置就相当于量测量，而速度我们是没法直接观察到的，它是需要用位置来计算求出来的，就属于间接测量的状态量。
- 量测矩阵 H \boldsymbol{H} H：上面讲到状态变量X是需要用量测量Z得到的，那么怎么得到的呢？就需要用H，因此H在这里表示的就是如何由量测量Z得到状态量X。
- V \boldsymbol{V} V为量测过程中的噪声， W \boldsymbol{W} W为系统噪声序列。
B、卡尔曼滤波经典的五个方程
{
X
^
k
/
k
−
1
=
Φ
k
,
k
−
1
X
^
k
−
1
P
k
/
k
−
1
=
Φ
k
,
k
−
1
P
k
−
1
Φ
k
,
k
−
1
T
+
Γ
k
−
1
Q
k
Γ
k
−
1
T
K
k
=
P
k
/
k
−
1
H
k
T
(
H
k
P
k
/
k
−
1
H
k
T
+
R
k
)
−
1
X
^
k
=
X
k
/
k
−
1
+
K
k
(
Z
k
−
H
k
X
^
k
/
k
−
1
)
P
k
=
(
I
−
K
k
H
k
)
P
k
/
k
−
1
(
I
−
K
k
H
k
)
T
+
K
k
R
k
K
k
T
\left\{\begin{array}{l} \hat\boldsymbol{X}_{k / k-1}=\boldsymbol{\Phi}_{k, k-1} \hat{\boldsymbol{X}}_{k-1} \\ \boldsymbol{P}_{k / k-1}=\boldsymbol{\Phi}_{k, k-1} \boldsymbol{P}_{k-1} \boldsymbol{\Phi}_{k, k-1}^{\mathrm{T}}+\boldsymbol{\Gamma}_{k-1} \boldsymbol{Q}_{k} \boldsymbol{\Gamma}_{k-1}^{\mathrm{T}} \\ \boldsymbol{K}_{k}=\boldsymbol{P}_{k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}}\left(\boldsymbol{H}_{k} \boldsymbol{P}_{k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}}+\boldsymbol{R}_{k}\right)^{-1} \\ \hat{\boldsymbol{X}}_{k}=\boldsymbol{X}_{k / k-1}+\boldsymbol{K}_{k}\left(\boldsymbol{Z}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k / k-1}\right)\\ \boldsymbol{P}_{k}=\left(\boldsymbol{I}-\boldsymbol{K}_{k} \boldsymbol{H}_{k}\right) \boldsymbol{P}_{k / k-1}\left(\boldsymbol{I}-\boldsymbol{K}_{k} \boldsymbol{H}_{k}\right)^{\mathrm{T}}+\boldsymbol{K}_{k} \boldsymbol{R}_{k} \boldsymbol{K}_{k}^{\mathrm{T}} \end{array}\right.
⎩⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎧X^k/k−1=Φk,k−1X^k−1Pk/k−1=Φk,k−1Pk−1Φk,k−1T+Γk−1QkΓk−1TKk=Pk/k−1HkT(HkPk/k−1HkT+Rk)−1X^k=Xk/k−1+Kk(Zk−HkX^k/k−1)Pk=(I−KkHk)Pk/k−1(I−KkHk)T+KkRkKkT
一下子看到这些公式，肯定慌，说不慌的估计都是数学大佬。但其实我们在应用中，并不一定需要去知道它是怎么推导的，或者去完全记住这些式子，更关键的是我们知道它是什么意义，然后会利用就可以了。
下面来一步步看看这五个公式是怎么把测量值及其误差范围以及预测值及其误差范围结合利用起来的：
理解第一步： 首先我们把这五个公式想成五个小工具，每一个工具都有自己的作用，我们的目标就是记住这五个工具的作用就行了。（这第一步叫做放松心里步骤）
理解第二步：从卡尔曼滤波的思想来明确我们需要哪些工具。首先我们的目的是结合系统推测的状态值、误差分布以及测量值、误差分布来得到一个更准确的值和一个更准确的误差分布。那么这里面就有两个小工具啦，第一结合状态值和测量值得到更准确的值。第二结合状态的误差分布和测量值的误差分布得到更准确的误差分布。
- 第一个工具（求当前时刻状态值）对应了我们的公式： X ^ k = X k / k − 1 + K k ( Z k − H k X ^ k / k − 1 ) \hat{\boldsymbol{X}}_{k}=\boldsymbol{X}_{k / k-1}+\boldsymbol{K}_{k}\left(\boldsymbol{Z}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k / k-1}\right) X^k=Xk/k−1+Kk(Zk−HkX^k/k−1),这个公式又叫做状态估值计算方程。作用就是由当前系统推测值结合测量值计算出当前时刻最优的状态值。仔细看可以发现，它主要分成两部分，左边部分 X k / k − 1 \boldsymbol{X}_{k / k-1} Xk/k−1就代表了当前系统的推测值，右边部分中的 Z k \boldsymbol{Z}_{k} Zk就是测量值，通过 K k ( Z k − H k X ^ k / k − 1 ) \boldsymbol{K}_{k}\left(\boldsymbol{Z}_{k}-\boldsymbol{H}_{k} \hat{\boldsymbol{X}}_{k / k-1}\right) Kk(Zk−HkX^k/k−1)这么一个式子，提取出 Z k \boldsymbol{Z}_{k} Zk中可用的信息，然后加到系统的推测值上，具体为什么可以用那个式子得到，我还没仔细研究，好像可以用贝叶斯的方法来推导（本来卡尔曼也是一种贝叶斯思想）
- 第二个工具（求当前时刻的误差分布）对应了我们的公式 P k = ( I − K k H k ) P k / k − 1 ( I − K k H k ) T + K k R k K k T \boldsymbol{P}_{k}=\left(\boldsymbol{I}-\boldsymbol{K}_{k} \boldsymbol{H}_{k}\right) \boldsymbol{P}_{k / k-1}\left(\boldsymbol{I}-\boldsymbol{K}_{k} \boldsymbol{H}_{k}\right)^{\mathrm{T}}+\boldsymbol{K}_{k} \boldsymbol{R}_{k} \boldsymbol{K}_{k}^{\mathrm{T}} Pk=(I−KkHk)Pk/k−1(I−KkHk)T+KkRkKkT.这个公式叫做估计均方误差方程。妈呀真长，同样的，我们把它当工具，不深入，嘻嘻！里面有两个需要注意，一是 P k / k − 1 \boldsymbol{P}_{k / k-1} Pk/k−1，这个代表着系统推测出的误差分布，还有一个凭空冒出的 R k \boldsymbol{R}_{k} Rk,这个其实是量测噪声的方差矩阵（应该在状态方程那里提到的，但是公式太多，搞不过来了，不好意思嘻嘻）。所以这个公式也是结合当前系统推测的和量测的得到更好的。
- 现在还剩三个工具， 其中有两个公式，作用相近，也就是 X ^ k / k − 1 = Φ k , k − 1 X ^ k − 1 \hat\boldsymbol{X}_{k / k-1}=\boldsymbol{\Phi}_{k, k-1} \hat{\boldsymbol{X}}_{k-1} X^k/k−1=Φk,k−1X^k−1和 P k / k − 1 = Φ k , k − 1 P k − 1 Φ k , k − 1 T + Γ k − 1 Q k Γ k − 1 T \boldsymbol{P}_{k / k-1}=\boldsymbol{\Phi}_{k, k-1} \boldsymbol{P}_{k-1} \boldsymbol{\Phi}_{k, k-1}^{\mathrm{T}}+\boldsymbol{\Gamma}_{k-1} \boldsymbol{Q}_{k} \boldsymbol{\Gamma}_{k-1}^{\mathrm{T}} Pk/k−1=Φk,k−1Pk−1Φk,k−1T+Γk−1QkΓk−1T这两个公式的作用都是为了满足卡尔曼滤波的迭代思想，分别称之为状态一步预测方程和一步预测均方差方程 。比如当前时刻的系统推测值，需要结合上一时刻的卡尔曼滤波的结果值所得到。同样的误差分布也是。那么最后一个工具， K k = P k / k − 1 H k T ( H k P k / k − 1 H k T + R k ) − 1 \boldsymbol{K}_{k}=\boldsymbol{P}_{k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}}\left(\boldsymbol{H}_{k} \boldsymbol{P}_{k / k-1} \boldsymbol{H}_{k}^{\mathrm{T}}+\boldsymbol{R}_{k}\right)^{-1} Kk=Pk/k−1HkT(HkPk/k−1HkT+Rk)−1我们可以发现，其它公式的大部分都有 K k \boldsymbol{K}_{k} Kk,叫做滤波增益方程。那么这个东西，可以把它理解为卡尔曼滤波思想进行融合的指标或者叫估计准则，也就是系统的推测和量测应该怎么结合才能让误差最小呢，就是通过它来体现的，它能找到那个让误差最小的方式。
四、卡尔曼滤波进阶
引导语：上面的公式是针对线性系统的，但是大部分系统都是非线性的，那怎么办呢？于是有了扩展卡尔曼滤波（EKF），无迹卡尔曼滤波（UKF ），然而这两个又是假设噪声是高斯分布的，那么噪声不是高斯分布的时候呢？于是又有了粒子滤波（PF）。并且传统的卡尔曼滤波还要求统计特性明确，计算中也牵涉到矩阵求逆的过程。
简单概括（快速认识）：
- 扩展卡尔曼滤波（EKF）的思想在于把非线性系统进行局部线性化，主要通过泰勒展开的方式来进行局部展开，然后再进行卡尔曼滤波
一个GPS静态滤波的卡尔曼滤波代码
（数据就没法提供了，尊重老师的成果）
%% 导入数据
%数据格式为：(时间，经度，纬度，高度)
GPS_data_static=importdata('./GPS_result_static.txt');
Z_global=GPS_data_static(:,2:4);
%% 将地理坐标系转换为空间直角坐标系
B1=Z_global(:,1)*pi/180; L1=Z_global(:,2)*pi/180; H1=Z_global(:,3);
a=6378137; b=6356752.3142;%WGS-1984
e=sqrt((a^2-b^2)/a^2); M1=a./sqrt((1-e^2*(sin(L1).^2)));
X1=(M1+H1).*cos(L1).*cos(B1);
Y1=(M1+H1).*cos(L1).*sin(B1);
Z1=(M1.*(1-e^2)+H1).*sin(L1);
Z_coor=[X1 Y1 Z1];
%% 系统方程的建立
%方程：X_=ΦX+U+ΓW，Φ是系统转移矩阵，Γ是系统噪声驱动阵，W是系统噪声
%W的协方差矩阵为Q
%设置Φ'
tao=[0.01,0.01,0.01];
T=1;
phi=diag([1,1,1]);
%设置系统噪声协方差阵Q
Q=zeros(3,3);
%% 观测方程的建立
%方程：Z=HX+V，H为观测矩阵，V为噪声
%V的协方差矩阵为R
H=diag([1 1 1]);
R=diag([25,25,25]);
%% 运用卡尔曼滤波迭代更新
%维度：H(3*12) X(12*1) Z(3*1) Q(12*12) P(12*12) R(3*3)
%--------------循环初值设定----------------
X=Z_coor(1,:)';
P=diag([25,25,25]);
X_result=zeros(size(X,1),size(Z_coor,1));
P_diag_result=zeros(size(P,1),size(Z_coor,1));
for i=1:size(GPS_data_static,1)
%----------------取出Z值-------------------
Z_next=Z_coor(i,:)';
%----------------时间更新------------------
%状态一步预测方程
X_pre=phi*X;
%一步预测均方差方程
P_pre=phi*P*phi'+Q;
%----------------量测更新-------------------
%滤波增益方程
K_next=P_pre*H'/(H*P_pre*H'+R);
%状态估值计算方程
X_next=X_pre+K_next*(Z_next-H*X_pre);
%估计均方差方程
P_next=(eye(3,3)-K_next*H)*P_pre;%I不能用1来替代，I只有对角线有值，而1相当于全都有
%结果保存
X_result(:,i)=X_next;
P_diag_result(:,i)=diag(P_next);
%结果更新（从代码角度本来可以不这么绕着写，但是这样更方便理解）
X=X_next;
P=P_next;
end
%% 将空间直角坐标系转换为地理坐标
X2=Z_coor(:,1); Y2=Z_coor(:,2);Z2=Z_coor(:,3);
P2=sqrt(X2.^2+Y2.^2);theta=atan((Z2*a)./(P2*b));e_p=sqrt((a^2-b^2)/b^2);
B2=90-atan(X2./Y2)/pi*180;
L2=180-atan((Z2+e_p^2*b*sin(theta).^3)./(P2-e^2*a*cos(theta).^3))/pi*180;
M2=a./sqrt((1-e^2*(sin(L2/180*pi).^2)));
H2=-P2./cos(L2/180*pi)-M2;
figure,plot(L2),figure,plot(Z_global(:,2))
%% 绘图
figure,plot(Z_coor(:,1),Z_coor(:,2)),hold on ,plot(X_result(1,:),X_result(2,:)),xlabel('纬度/^。 ','FontSize',18),ylabel('经度/^。 ','FontSize',18)
,legend('滤波前','滤波后')
figure,plot(Z_global(:,1)),hold on,plot(B2),xlabel('采样点','FontSize',18),ylabel('纬度/^。 ','FontSize',18),legend('滤波前','滤波后')
figure,plot(Z_global(:,2)),hold on,plot(L2),xlabel('采样点','FontSize',18),ylabel('经度/^。 ','FontSize',18),legend('滤波前','滤波后')
figure,plot(Z_global(:,3)),hold on,plot(H2),xlabel('采样点','FontSize',18),ylabel('高度（m） ','FontSize',18),legend('滤波前','滤波后')
figure,plot(P_diag_result(1,:)),xlabel('采样点','FontSize',18),ylabel('估计均方差 ','FontSize',18)
扫一扫
3869
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
