卡尔曼滤波器

之前玩飞控就接触过卡尔曼滤波器，但一直没有专心研究，这几天有空沉下心来好好研究了一下，但是也查找了许多资料才慢慢了解，这里我想站在一个零基础的学习者角度，将所有我看过的资料进行整理，并加上我的理解，自述成文，于是便有了这篇文章。(本文中红色字体部分表示有待深入了解)
(该文章会持续更新，直到这句话消失，毕竟对于卡尔曼滤波的持续了解，一定会有新的不同的认识，我也会及时补充，改正；也欢迎大家在评论区留下你的意见，建议，表述错误或表述不清楚的地方，谢谢)

---

---

第一部分：需要了解的基础知识

了解卡尔曼滤波器需要掌握的一些基础知识，如果已经有了深入的了解可以直接跳转到第二部分

高斯分布：即正态分布，他有两个重要参数，分别是均值$\mu$和方差${\sigma }^2$；
若一个随机变量$x$的分布服从如下分布：其分布曲线是钟形曲线，如下图所示，以中值为分界线，左右对称分布或近似对称分布，则该分布便是高斯分布

图中绿色曲线便是概率密度曲线，即分布曲线(密度函数如下所示)，最高点对应的随机变量$x$的值便是该分布的均值$\mu$，分布曲线主体的宽度反映该分布的方差大小，曲线主体宽度越宽，表示其分布越离散，方差越大；如下图所示：
$$f(x)=\frac{1}{\sqrt{2\pi }\sigma }{e}^{-\frac{(x-\mu {)}^2}{2{\sigma }^2}}$$
为什么要使用高斯分布？世界上存在着许多不同的概率分布，而最广泛使用的就是高斯分布了，比如人群身高，鞋码，工作时间等等；高斯分布能够反映自然-社会科学中大多数分布的普遍规律，著名试验–高尔顿钉板试验便是很好的例证，其实验结果如下：

这里着重强调的是：测量误差等许多误差数据均是服从高斯分布的，且分布均值通常等于或靠近零($\mu \approx 0$)，即可表示为：$N=(0,{\sigma }^2)$；（测量值与实际值之间的差值叫误差）

方差：
用于衡量单个随机变量的分布误差，即离散程度。方差越大表示单个随机变量样本的分布越离散，方差越小表示分布越集中；
方差分为样本方差和总体方差，关于两者的区别和计算公式可以参考【计算样本方差时为什么是除以（n-1）？】；
本文统一使用总体方差，公式如下：
$${\sigma }^2=\frac{1}{n-1}\sum _{i=1}^n(x_i-\overline{x}{)}^2$$

协方差：
用于衡量两个随机变量的总体误差(公式理解)和相关性(随后讲到)；方差是协方差的一种特殊情况：当随机变量$x$和$y$为同一随机变量时，协方差便表示方差，即下面公式中，若$y=x$，则有$Cov(x,y)=Cov(x,x)={\sigma }_x^2$
$$Cov(x,y)=\frac{1}{n-1}\sum _{i=1}^n(x_i-\overline{x})(y_i-\overline{y})$$

通过上述公式，我们很容易理解协方差反映两个变量的总体误差；而对于相关性，在参考文章《协方差和相关性的意义》后，可以得出如下结论：
1.$x$变大，同时$y$也变大，说明两个变量是同向变化的，这时协方差就是正的。
2.$x$变大，同时$y$变小，说明两个变量是反向变化的，这时协方差就是负的。
3.从数值来看，协方差的数值越大，两个变量同向程度也就越大，反之亦然。
即:
$Cov(x,y)>0:$ 表示两者正相关
$Cov(x,y)<0:$ 表示两者负相关
$Cov(x,y)=0:$ 表示两者不相关

列举协方差的几个性质：
（1）$Cov(x,y)=Cov(y,x)$；
（4）$Cov(x+a,y+b)=Cov(x,y)$，其中$a,b$为常数；
（3）$Cov(x_1+x_2,y)=Cov(x_1,y)+Cov(x_2,y)$;

协方差矩阵
前面讲的高斯分布仅仅只涉及到一个变量$x$，故只有一个维度，仅用单个轴线便可表示概率密度分布，均值用$\mu$表示，方差用${\sigma }^2$表示，该高斯分布表示为$N=(\mu ,{\sigma }^2)$

若存在两个均服从高斯分布的变量$x,y$时，便需要使用两个轴在二维空间中表示

对应于一维空间的均值和方差，二维空间分布同样需要用这两个参数表示，但表述方式会有所不同：
1.由于两个高斯分布的均值轴线只存在一个交点，故二维空间存在唯一的点(即该交点)表示均值$\mu =({\mu }_x,{\mu }_y)$；
2.与一维方差不同，在二维中既要考虑两个变量各自的离散程度，即$Cov(x,x)$和$Cov(y,y)$，又要考虑两者的相关性，即$Cov(x,y)$；矩阵能很好的将这些量进行整合。因此我们可以得到如下矩阵：
$$\Sigma =\left[\begin{array}{cc}Cov(x,x)& Cov(x,y)\\ Cov(y,x)& Cov(y,y)\end{array}\right]$$

该矩阵便是协方差矩阵，它为对称矩阵，主对角线表示各轴的方差，其他位置表示各不同变量间的协方差。由此看出，协方差矩阵既能反映各变量单独的分布情况，又能反映变量间的相关性和总体误差，因此对应于一维空间的方差${\sigma }^2$，在二维空间中需要用协方差矩阵$\Sigma$表示；

故二维高斯分布便表示为$N=(\mu ,\Sigma )$
(二维高斯分布对应的协方差矩阵是$2\times 2$的矩阵，更高维度的高斯分布对应的协方差矩阵可参考: 协方差矩阵.)

看到这里我们便建立了以下关键信息：
1.测量误差等诸多误差（又称为噪声）大多服从均值为0的高斯分布；
2.一维高斯分布用$N=(\mu ,{\sigma }^2)$表示，其中$\mu$表示一个数值，${\sigma }^2$表示方差；
3.二维高斯分布用$N=(\mu ,\Sigma )$表示，其中$\mu$表示一个向量：$\mu =({\mu }_x,{\mu }_y)$，$\Sigma$表示协方差矩阵；

以上便是我对于这些知识的个人见解，欢迎在评论区进行批评指正
关于以上内容的更准确专业的解释，还需要自行翻书查阅！

---

---

第二部分：卡尔曼滤波原理

理解了上面的基础知识后，再来分析卡尔曼滤波原理会容易许多
首先来看看什么是卡尔曼滤波，度娘的解释为：

这里有两个使用条件需要着重强调：
1.线性系统：什么是线性呢？学过数学的都懂，它是这样子的：直线为线性，曲线为非线性；那什么是线性系统呢？这个放在后面讲解

2.动态系统：很好理解，即字面意思，通俗的讲就是状态在不断变化的系统

下面用过举例来说明卡尔曼滤波的实现原理：
这里采用网上使用最多的示例，小车例子来说明，先放图

我们的目的是测量轿车距左边测距点的当前距离$s$，和当前速度$v$，现在采用如下方式测量
1.用放在测距点的激光传感器来测量距离
2.用车上的编码盘来测量速度
我们构建一个状态矩阵$\left[\begin{array}{c}s\\ v\end{array}\right]$以反映车辆的状态
我们将$t$时刻传感器的测量值称为观测状态，对应的状态矩阵称为观测矩阵$z_t$：
$$z_t=\left[\begin{array}{c}{s}_t\\ v_t\end{array}\right]$$

该示例中进行卡尔曼滤波的数据对象有距离$s$和速度$v$；也就是说：状态矩阵中的元素均为卡尔曼滤波器的滤波对象

由于两种传感器的观测值均存在测量误差(我们称为观测噪声，用$r_t$表示)，我们默认该噪声服从高斯分布 (基础部分有提到：测量误差等诸多误差均是服从高斯分布的)，由于状态矩阵中有两个元素，因此观测噪声的高斯分布为二维高斯分布，协方差矩阵为$R_t$，即$N_{r_t}=(0,R_t)$，故观测状态也是服从高斯分布的：$N_{z_t}=({\mu }_{z_t},R_t)$ 其中${\mu }_{z_t}=(s_t,v_t)$。此处$R_t$也代表$t$时刻观测噪声对于观测状态分布的影响。

上面我们分析的是来自传感器的观测数据，下面我们从理论上进行分析
假设我们已知上一时刻(${}_{t-1}$时刻)卡尔曼滤波器输出的最优状态为${\hat{x}}_{t-1}=\left[\begin{array}{c}{\hat{s}}_{t-1}\\ {\hat{v}}_{t-1}\end{array}\right]$，现需要根据上一时刻的最优状态${\hat{x}}_{t-1}$来预测当前$t$时刻的预估状态(由于卡尔曼滤波共有两次估计过程，我们将第一次，也就是本次估计称为先验估计，将第二次估计称为后验估计。先验估计的结果会通过${}^{-}$表示(如${\hat{x}}^{-}$)，称为预估状态，后验估计结果则没有${}^{-}$(如$\hat{x}$)，称为最优状态)，通过运动学公式可得：(假设为匀速运动,不考虑外部因素)
$$\{\begin{array}{l}{\hat{s}}_t^{-}={\hat{s}}_{t-1}+{\hat{v}}_{t-1}\Delta t\\ {\hat{v}}_t^{-}={\hat{v}}_{t-1}\end{array}$$

输入量为上一时刻的状态${\hat{s}}_{t-1},{\hat{v}}_{t-1}$，输出量为当前时刻的状态${\hat{s}}_t^{-},{\hat{v}}_t^{-}$，公式表明输入量和输出量呈线性关系，故可写成矩阵形式：
$${\hat{x}}_t^{-}=\left[\begin{array}{c}{\hat{s}}_t^{-}\\ {\hat{v}}_t^{-}\end{array}\right]=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\hat{s}}_{t-1}\\ {\hat{v}}_{t-1}\end{array}\right]$$

我们得到了一个新的矩阵$\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]$，称为状态转移矩阵，它代表从一个状态到另一个状态的转换关系，由于不同的情况下每一次预测的状态转移矩阵都可能不同，故用$F_t$表示$t$时刻的状态转移矩阵，则上述状态变换公式可简化为：
$${\hat{x}}_t^{-}=F_t{\hat{x}}_{t-1}$$

我们来看看卡尔曼滤波器上一次输出的最优状态${\hat{x}}_{t-1}$
因：由于系统是存在噪声的(这是由系统迭代引起)，且服从高斯分布，因此卡尔曼滤波器输出的最优状态也是服从高斯分布的(不要问为什么，这是一个循环迭代的过程，如果不懂，先暂时建立这样一个思维)；即最优状态${\hat{x}}_{t-1}$服从高斯分布，需要用均值和协方差矩阵来表示，如下：
$$N_{{\hat{x}}_{t-1}}=({\hat{\mu }}_{t-1},P_{t-1})，其中\{\begin{array}{l}{\hat{\mu }}_{t-1}=({\hat{s}}_{t-1},{\hat{v}}_{t-1})\\ P_{t-1}={\Sigma }_{({\hat{s}}_{t-1},{\hat{v}}_{t-1})}=\left[\begin{array}{cc}Cov({\hat{s}}_{t-1},{\hat{s}}_{t-1})& Cov({\hat{s}}_{t-1},{\hat{v}}_{t-1})\\ Cov({\hat{v}}_{t-1},{\hat{s}}_{t-1})& Cov({\hat{v}}_{t-1},{\hat{v}}_{t-1})\end{array}\right]\end{array}$$
果：通过公式${\hat{x}}_t^{-}=F_t{\hat{x}}_{t-1}$计算得到的预估状态${\hat{x}}_t^{-}$同样服从高斯分布，原因在于该系统是线性系统，通过该线性公式，将${\hat{x}}_{t-1}$的高斯分布属性传递给了${\hat{x}}_t^{-}$；(由此可见，前面讲到的规定线性系统这个条件的目的就在于此)，故预估状态的高斯分布表示为：($P_t^{-}$公式推导)
$$N_{{\hat{x}}_t^{-}}=({\hat{\mu }}_t^{-},P_t^{-})，其中\{\begin{array}{l}{\hat{\mu }}_t^{-}=({\hat{s}}_t^{-},{\hat{v}}_t^{-})\\ P_t^{-}={\Sigma }_{({\hat{s}}_t^{-},{\hat{v}}_t^{-})}=F_t{P}_{t-1}{F}_t^T\end{array}$$

这个属性传递的因果关系可以用下面这幅图表示：蓝色表示上一次后验估计输出状态的高斯分布，紫色表示本次先验估计输出状态的高斯分布，两者的关系可用状态转移矩阵$F_t$表示(图中用$k$表示当前时刻$t$)

ok,上面的预测是未考虑外部影响因素的，而外部因素又分为确定因素和不确定因素
确定因素
可以用数学方法量化表示的因素，如加速度，摩擦力等，这些因素需要以表达式的形式加入到上述的状态预测公式中，例如我考们虑加速度$a$,假设司机油门固定，则做匀加速运动，因此预测方程将变为：
$$\{\begin{array}{l}{\hat{s}}_t^{-}={\hat{s}}_{t-1}+{\hat{v}}_{t-1}\Delta t+\frac{1}{2}a{\Delta t}^2\\ {\hat{v}}_t^{-}={\hat{v}}_{t-1}+a\Delta t\end{array}$$
转换为矩阵形式后将是：
$${\hat{x}}_t^{-}=\left[\begin{array}{c}{\hat{s}}_t^{-}\\ {\hat{v}}_t^{-}\end{array}\right]=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\hat{s}}_{t-1}\\ {\hat{v}}_{t-1}\end{array}\right]+\left[\begin{array}{c}\frac{{\Delta t}^2}{2}\\ \Delta t\end{array}\right]a$$
增加的矩阵$\left[\begin{array}{c}\frac{{\Delta t}^2}{2}\\ \Delta t\end{array}\right]$称为状态控制矩阵，表示a这个控制量是如何影响到预测状态的，用$B_t$表示，然而后面压根没用到…

不确定因素
如地面平整度，不确定的风速，司机踩油门时出现的上下抖动等无法进行准确的跟踪和量化，会对系统产生不确定干扰的因素，统称为过程噪声(区别于观测噪声$r_t$)，用符号$w_t$表示

实际上我们需要将这些因素都考虑进去，因此完整的先验估计公式应该是：
$${\hat{x}}_t^{-}=F_t{\hat{x}}_{t-1}+B_{t}a+w_t$$
加入外部因素后，预估状态的高斯分布将发生变化，即均值和协方差矩阵均需要更新，我们将$t$时刻过程噪声对于预测状态分布的影响用矩阵$Q_t$表示，其维度与$P_t^{-}$一致，故更新为：(加法作用)
$$P_t^{-}=F_t{P}_{t-1}{F}^T+Q_t$$
怎么理解呢？看下面这幅图：

由公式可知：$Q_t$对于最终值$P_t^{-}$的影响是在原$P_t^{-}$的基础上叠加而来；
上图蓝色部分表示上一次后验估计最优结果的高斯分布，紫色部分表示未考虑到噪声时的预估状态的高斯分布，而绿色圈(忽略圈内的箭头)表示噪声对于先验估计结果的影响，由于其具有不确定性，故可能分布于不同的地方，进行叠加后得到如下：(可以看出预估状态的分布更广，这正是过程噪声的影响的结果)

不用急，慢慢消化

传感器的原始数据通常不是我们实际需要的数据(包括单位)，需要经过一个转换(包括单位换算)，举一个例子：预测的数据单位是cm，但传感器读取的数据单位是mm(我们假定单位为cm的数据空间称为预测空间，单位为mm的数据空间称为观测空间)，若要将预测状态的单位转换为与传感器观测状态的单位一致，即将预测空间数据转换到观测空间，则需要在预测数据上×10，因此可以写出如下转换关系
$${\hat{z}}_t^{-}=\left[\begin{array}{c}{\hat{s}}_t^{-}\times 10\\ {\hat{v}}_t^{-}\times 1\end{array}\right]=\left[\begin{array}{cc}10& 1\end{array}\right]\left[\begin{array}{c}{\hat{s}}_t^{-}\\ {\hat{v}}_t^{-}\end{array}\right]=\left[\begin{array}{cc}10& 1\end{array}\right]{\hat{x}}_t^{-}$$

${\hat{z}}_t^{-}$表示预估状态经过数据空间转换后的结果，你可以理解为将预测空间的预估状态做了单位转换后得到观测空间的预估状态；
我们将新的矩阵$\left[\begin{array}{cc}10& 1\end{array}\right]$称为状态空间转换矩阵，$t$时刻下用$H_t$表示，下图表明了$t$时刻(图中用$k$表示)预测空间与状态空间的映射关系：

将各状态统一到观测空间后，我们期望的测量数据应该是：
$$\{\begin{array}{l}{z}_{t-expected}=H_t{\hat{x}}_t^{-}\\ {\Sigma }_{t-expected}=H_t{P}_t^{-}{H}_t^T\end{array}$$
但实际观测数据受到测量噪声$r_t$的影响：(叠加作用)
$$\{\begin{array}{l}{z}_t=z_{t-expected}+r_t\\ {\Sigma }_t={\Sigma }_{t-expected}+R_t\end{array}$$

---

到此，我们来理一理几个关键点
1.当前$t$时刻观测状态$z_t=\left[\begin{array}{c}{s}_t\\ v_t\end{array}\right]$，其高斯分布受到观测噪声$r_t$的影响，观测噪声协方差为$R_t$
2.先验估计预估状态${\hat{x}}_t^{-}=\left[\begin{array}{c}{\hat{s}}_t^{-}\\ {\hat{v}}_t^{-}\end{array}\right]$，其高斯分布受到过程噪声$w_t$的影响，过程噪声协方差矩阵为$Q_t$
3.两个状态空间的转换关系为：$H$（在进行后续处理时，需要将各个状态转换为同一空间）
4.观测状态与观测空间的预估状态的关系为：
$$\{\begin{array}{l}{z}_t=H_t{\hat{x}}_t^{-}+r_t\\ {\hat{x}}_t^{-}=F_t{\hat{x}}_{t-1}+B_{t}a+w_t\end{array}$$

我们将预估状态与观测状态进行空间统一后放在同一概率密度图中，如下：

卡尔曼滤波的关键就在于：如何通过同一状态空间下的观测状态高斯分布$N_{z_t}$ 和 预估状态高斯分布$N_{{\hat{x}}_t^{-}}$，来得出最优状态

看到这里，我当时很自然的就想到，既然叠加图中两种颜色分别为两种高斯分布的概率密度，那要想得到最优估计，不就是两者重叠的地方嘛，但仔细一想，并非这样，若交叉部分刚好是两者最暗的部分呢，可能两者叠加的亮度还没有其他位置亮，当然就不是最优估计了，所以事情并没有这么简单！

---

网上很多是直接给出两个正态分布乘积(预估正态分布和观测正态分布)得到的结果作为最优估计，但却没说明为什么要这样做，后续还需要继续了解

两个正态分布的乘积依然是高斯分布(不知道的百度)，那我用上面两个高斯分布模型相乘后得到的均值就是我们卡尔曼滤波器最终输出的最优状态了。

分割线 - 下面内容不完整，有待整理

下面来单独推导一下这个过程：
正态分布的概率密度函数为：
$$f(x)=\frac{1}{\sqrt{2\pi }}$$

假设对于单个变量，有两个独立的正态分布$N_1(x,{\mu }_1,{\sigma }_1^2)$，$N_2(x,{\mu }_2,{\sigma }_2^2)$，则$N_1\times N_2$为

由此可得到新的分布服从如下$N(\mu ,{\sigma }^2)$的正态分布

此时我们令

简化公式后得到：

以上是单个变量的情况，而多变量的情况下方差${\sigma }^2$则变为协方差矩阵$\Sigma$形式，如下：

以上便是卡尔曼滤波公式中最后三个公式的推导，且是各变量均处于同一空间中的推导，如何理解这句话呢，你可以类似地理解为各变量单位相同。
在我们这个例子中这两个正态分布分别为观测模型正态分布和预测模型正态分布，首先我们需要将两者转为同一空间，这里同一转化到观测空间，还记得预测状态空间到观测状态空间的转换关系矩阵$H$嘛！

假设预测模型在观测空间下的正态分布为$N_1({\mu }_1,{\Sigma }_1)$；
假设观测模型在观测空间下的正太分布为$N_2({\mu }_2,{\Sigma }_2)$；
则转换关系如下：
$N_1({\mu }_1,{\Sigma }_1)$ = $(H{\hat{x}}_t^{-},H{P}_t^{-}{H}^T)$；
$N_2({\mu }_2,{\Sigma }_2)$ = $(z_t,R)$;
转换完成后将$N1,N2$带入上述推导过程，最终得到：

卡尔曼滤波器除了有滤波预测功能外，还有数据融合功能(这个需要用其他例子来说明，后续会补上)

---

---

第三部分：Matlab代码示例

%% 卡尔曼滤波器示例1:匀速运动
clc,clear
%--------------------------------------------------------------------------
%---------------------------------数据构造---------------------------------
%--------------------------------------------------------------------------
t = 0.1;                        	%时间间隔

s = 1:300;                          %理想位置
s_noise = 25*randn(1,length(s));	%位置观测噪声
s_m = s + s_noise;                  %实际观测值

v = ((max(s)-min(s))/(length(s)*t))*ones(1,length(s));
v_noise = 2*randn(1,length(v));
v_m = v + v_noise;

z = [s_m;...
     v_m];                          %观测状态集矩阵 [位置;速度]
%--------------------------------------------------------------------------
%---------------------------------变量初始化-------------------------------
%--------------------------------------------------------------------------
x = [0;...
     0];        %预测状态矩阵 [位置;速度]
P = [1 0;...
     0 1];    	%预测状态协方差矩阵
F = [1 t;...
     0 1];    	%状态转移矩阵
H = [1 0;...
     0 1];   	%状态空间转换矩阵

%--------------------------------------------------------------------------
%-----------------------------调整参数-噪声协方差--------------------------
%--------------------------------------------------------------------------
%外部噪声对预测状态分布的影响协方差矩阵
Q = [0.0001 0;...
    0      0.00001]; 
%过程噪声对观测状态分布的影响协方差矩阵
R = [0.05 0;...
     0   0.005];                  
 
%--------------------------------------------------------------------------
%-----------------------------初始化结果记录变量---------------------------
%--------------------------------------------------------------------------
S = zeros(1,length(s_m));
V = zeros(1,length(v_m));

%--------------------------------------------------------------------------
%-----------------------------------开始滤波-------------------------------
%--------------------------------------------------------------------------
tic
for i = 1:length(s_m)
    %先验估计
    x_ = F * x;
    P_ = F*P*F' + Q;
    %卡尔曼增益
    K = H*P_*H'/(H*P_*H' + R);
    %更新
    x = x_ + K*(z(:,i)-H*x_);% 得到当前状态的最优化估算值  增益乘以残差
    S(i) = x(1);
    V(i) = x(2);
    P = P_*(eye(2)-K*H);%更新K状态的协方差
end
toc
%--------------------------------------------------------------------------
%-----------------------------------显示结果-------------------------------
%--------------------------------------------------------------------------
x_axis = t:t:t*length(s);    %x轴表示时间
zoom on;            %开启图形缩放功能
set(gcf,'Position',[200 200 1500 600]);
subplot(1,2,1);plot(x_axis,s_m, x_axis,s, x_axis,S);
legend('观测','理想','预测');title('距离');xlabel('T');ylabel('S');
subplot(1,2,2);plot(x_axis,v_m, x_axis,v, x_axis,V);
legend('观测','理想','预测');title('速度');xlabel('T');ylabel('V');

结果如下：

%% 卡尔曼滤波器示例(匀加速运动)
clc,clear
%--------------------------------------------------------------------------
%---------------------------------数据构造---------------------------------
%--------------------------------------------------------------------------
sigma1 = 5;
sigma2 = 1;

s = zeros(1,100);                   %初始化理想位置
s_noise = sigma1*randn(1,length(s));%位置观测噪声
s_m = zeros(1,length(s));           %位置观测值
                        
v = ones(1,length(s));              %初始化理想速度(初始速度设为1)
v_noise = sigma2*randn(1,length(v)); %速度观测噪声
v_m = ones(1,length(v));        	%初始化速度观测值

z = [s_m;...
     v_m];                          %观测状态集矩阵 [位置;速度]

%--------------------------------------------------------------------------
%---------------------------------变量初始化-------------------------------
%--------------------------------------------------------------------------
t = 0.1;        %固定采样间隔
a = 0.5;        %固定加速度(将加速度设为0，再给定不为0的初始速度即为匀速运动示例)

x = [0;...
     0];        %预测状态矩阵 [位置;速度]
P = [1 0;...
     0 1];    	%预测状态协方差矩阵
F = [1 t;...
     0 1];    	%状态转移矩阵
H = [1 0;...
     0 1];   	%状态空间转换矩阵

%--------------------------------------------------------------------------
%-----------------------------调整参数-噪声协方差--------------------------
%--------------------------------------------------------------------------
%外部噪声对预测状态分布的影响协方差矩阵
Q = [0.06 0;...
    0      0.06]; 
%过程噪声对观测状态分布的影响协方差矩阵
R = [sigma1 0;...
     0   sigma2];                  
 
%--------------------------------------------------------------------------
%-----------------------------初始化结果记录变量---------------------------
%--------------------------------------------------------------------------
S = zeros(1,length(s_m));
V = zeros(1,length(v_m));

%--------------------------------------------------------------------------
%-----------------------------------开始滤波-------------------------------
%--------------------------------------------------------------------------
tic
for i = 1:length(z)
    if i ~= 1
        s(i) = s(i-1) + v(i-1)*t + 0.5*a*t.^2;  %本次理想位置
        v(i) = v(i-1) + a*t;                    %本次理想速度  
    end
    s_m(i) = s(i) + s_noise(i);             %本次测量位置
    v_m(i) = v(i) + v_noise(i);             %本次测量速度
    z(:,i) = [s_m(i);v_m(i)];               %本次测量状态
    
    %先验估计
    x_ = F * x;
    P_ = F*P*F' + Q;
    %卡尔曼增益
    K = H*P_*H'/(H*P_*H' + R);
    %更新
    x = x_ + K*(z(:,i)-H*x_);
    P = P_*(eye(2)-K*H);
    
    S(i) = x(1);
    V(i) = x(2);
end
toc

%--------------------------------------------------------------------------
%-----------------------------------显示结果-------------------------------
%--------------------------------------------------------------------------
x_axis = t:t:t*length(s);    %x轴表示时间
zoom on;            %开启图形缩放功能
set(gcf,'Position',[200 200 1500 600]);
subplot(1,2,1);plot(x_axis,s_m, x_axis,s, x_axis,S);
legend('观测','理想','预测');title('距离');xlabel('T');ylabel('S');
subplot(1,2,2);plot(x_axis,v_m, x_axis,v, x_axis,V);
legend('观测','理想','预测');title('速度');xlabel('T');ylabel('V');

参考链接：

1. LaTeX 特殊符号、加帽子符号、横线和波浪线
2. 为什么方差计算公式中除以n-1
3. 说说卡尔曼滤波
4. 卡尔曼滤波的原理、理解与仿真
5. Matlab学习视频