卡尔曼滤波器原理讲解及其matlab实现

请叫我7plus

已于 2022-04-13 11:41:12 修改

阅读量1.7w

点赞数 25

分类专栏：雷达信号处理文章标签：算法 matlab

于 2022-04-11 20:48:56 首次发布

本文链接：https://blog.csdn.net/weixin_41544435/article/details/124107741

版权

雷达信号处理专栏收录该内容

4 篇文章 15 订阅

订阅专栏

引言：在进行一些信号处理的过程中，我们通常会采集到一些数据，但是实际测量到的数据是受到噪声干扰了之后的，故与真实的数据有一些偏差。因此我们把 [ 通过测量数据进行一系列处理得到近似于真实数据 ] 的过程( 或利用测量数据估计得到近似于真实数据的过程 )称为 [ 估计 ] 。

其中 [ 波形估计 ] 也是一种估计，它是估计的一个波形( 即一系列数据 ),是很重要的一种工具，在目标跟踪，轨迹跟踪等方面具有很重要的应用。一般波形估计是通过一组数据得到另一组数据，就像是一组数据通过了一个滤波器，得到另一组数据。故经常会听到 [维纳滤波器]、[卡尔曼滤波器] 等词汇。

本文讲解卡尔曼滤波器，连续的卡尔曼滤波器在计算机应用中没有多大意义，故一般说到的卡尔曼滤波器都是离散卡尔曼滤波器。

一：卡尔曼滤波器的信号模型[1-2]

卡尔曼滤波器的信号模型是由状态方程和观测方程组成的，具体的纯公式推导不太好理解，故在此直接用例子来说明：

设目标匀加速度 $a$ 从原点开始作直线运动，考虑到加速度可能会受到时变扰动，写出该例子的信号模型。

状态方程：本例子中，目标距离 $r$ 、目标速度 $v$ 、加速度 $a$ 为三个状态，其中加速度受到的时变扰动为 $w$ 。有：

$r_{k}=r_{k-1}+T\cdot v_{k-1}+\frac{T^{2}}{2}\cdot a_{k-1}$

$v_{k}=v_{k-1}+T\cdot a_{k-1}$

$a_{k}=a_{k-1}+w_{k-1}$

符号解释：

$r_{k}$ 代表k时刻的距离， $r_{k-1}$ 代表k-1时刻的距离。其他的类似，卡尔曼滤波器就是不断利用前一时刻的值去估计下一时刻，不断地递推；

$w_{k-1}$ 表示在k-1时刻目标运动加速度收到的扰动噪声 ( 在这里我们可以认为在路面上匀加速的车受到了风的吹动，这个风就是噪声 ) 。

将上式写成矩阵形式如下：

$\begin{bmatrix} r_{k}\\ v_{k}\\ a_{k}\end{bmatrix}=\begin{bmatrix}1 &T & \frac{T^{2}}{2}\\ 0 & 1 & T\\ 0 & 0 & 1\end{bmatrix}\begin{bmatrix} r_{k-1}\\ v_{k-1}\\ a_{k-1}\end{bmatrix}+\begin{bmatrix} 0\\ 0\\ 1\end{bmatrix}w_{k-1}$

令 $\overrightarrow{s_{k}}$ 表示 $k$ 时刻目标运动的三个状态量 $r_{k},v_{k},a_{k}$ 构成三位状态矢量。

$\overrightarrow{s_{k}}=\begin{bmatrix}r_{k}\\ v_{k}\\ a_{k}\end{bmatrix},\overrightarrow{\Phi _{k,k-1}}=\begin{bmatrix}1 &T & \frac{T^{2}}{2}\\ 0 & 1 & T\\ 0 & 0 & 1\end{bmatrix},\overrightarrow{\Gamma _{_{k-1}}}=\begin{bmatrix} 0\\ 0\\ 1\end{bmatrix}$

符号解释：

$\overrightarrow{\Phi _{k,k-1}}$ 表示一步状态转移矩阵。代表k时刻的状态可由前一时刻的状态，并考虑噪声矢量推的；

$\overrightarrow{\Gamma _{k-1}}$ 表示控制矩阵。反映了k-1时刻扰动噪声矢量对系统状态矢量影响程度的矩阵。

写成矩阵形式有：

$\overrightarrow{s_{k}}=\overrightarrow{\Phi _{k,k-1}}\cdot \overrightarrow{s_{k-1}}+\overrightarrow{\Gamma _{k-1}}\cdot \overrightarrow{w_{k-1}}$

以上状态方程说明了，可以由前一时刻推的后一时刻的状态，故该式又被称作 [ 一步状态递推公式 ] 。换句话说：我们也可以根据该时刻(k)的状态值去预测下一时刻(k+1)状态值，因为是[预测值]所以不准确，故需要 [ 修正项 ] 去修正,而预测值怎么得来就和下面的 [ 观测值 ] 有关系了。

（还需要注意的一点是，在上述的状态方程中都是确定值，或者说实际值，但是实际当中我们是不知道实际值的，我们需要的就是实际值，所以实际应用中用的状态值都是估计值）

下面来看观测方程，虽然观测值我们是直接得到具体的值，但是因为已经用状态矢量 $\overrightarrow{s_{k}}$ 来表示各状态变量 $r_{k},v_{k},a_{k}$ ，所以在观测方程当中只能用状态矢量 $\overrightarrow{s_{k}}$ 。这样在直接测距情况下的目标运动观测方程为：

$x_{k}=\overrightarrow{H_{k}}\cdot \overrightarrow{s_{k}}+n_{k}$

其中：

$H_{k}=[1,0,0]$ ，代表观测矩阵，是已知的，随着不同场景发生改变。

$x_{k}$ 就是k时刻的运动目标距离测量数据；

$n_{k}$ 是观测噪声。

上述也说了观测值要作为修正项，具体怎么修正，还需要一些条件。

二：其他方程及变量介绍

1、状态滤波： $\overrightarrow{s_{k}}=\overrightarrow{\Phi _{k,k-1}}\cdot \overrightarrow{s_{k-1}}+\overrightarrow{K_{k}}\cdot (\overrightarrow{x_{k}}-\overrightarrow{H_{_{_{k}}}}\cdot \overrightarrow{\Phi _{k,k-1}}\cdot \overrightarrow{s_{k-1}})$

该公式类似于状态方程（不考虑噪声），但是却多了等式右边的第二项。上文也提到了，上式右边第一项是预测值，不准确，因此需要修正项去修正。

其中：

$\overrightarrow{s_{k}}$ ，本时刻经过修正之后的值（或者称作状态滤波值，最终估计值）。我们最终得到的就是这个。

$\overrightarrow{K_{k}}$ 是卡尔曼增益，后面给出计算公式。

2、状态滤波的均方误差阵 $M_{k}$ ，反映了状态滤波的精度。用来计算卡尔曼增益。

3、状态一步预测：1式中的右边第一项可以写成： $\overrightarrow{s_{k+1|k}}=\overrightarrow{\Phi} \cdot \overrightarrow{s_{k}}$ 。其中， $\overrightarrow{s_{k+1|k}}$ 是根据本时刻的滤波值去预测下一时刻的值得到的 [ 状态一步预测值 ]，这里也进一步细说了预测值和最终估计值的符号的不同。

4、状态一步预测的均方误差阵 $M_{k|k-1}$ 。反映了一步预测值的精度。用来计算卡尔曼增益。

三：卡尔曼滤波器递推公式

上述也讲述了很多关于卡尔曼滤波器的公式，为了更直观，给出下图的公式表：

上图中的大部分公式已经提过，其中的符号也都解释了。下面讲述各分量在递推过程中的关系，下面看图：

该图其实可以分成两部分观看，第一部充（Ⅰ）（Ⅱ）（Ⅲ）是不断递推求卡尔曼增益的过程。第二部分（Ⅳ）（Ⅴ）是修正和预测下一步的过程。

还需要注意一点的是： $\overrightarrow{s_{0}}$ 和 $\overrightarrow{M_{0}}$ 是需要自己设定的。可以参考下图：

四：matlab仿真[3]

上述所讲述的是状态为矢量的卡尔曼滤波器。在这里仿真了一个状态为标量时的离散卡尔曼滤波器，标量状态的卡尔曼滤波器就比矢量的简单得多了，不用考虑什么矩阵矢量了就是将上述的都变成标量。

温度测量：

仿真图：

其中黑点是温度在24摄氏度上下漂移的结果，是测量值，经过卡尔曼滤波器之后得到蓝色的线即状态滤波结果。可以看到很不戳！

代码：

%定义超参数
Q=4e-4;
R=0.25;

%定义观测数据维数
mn=[4000,1];

%仿真测量值
X=24+sqrt(R)*randn(mn);

%定义迭代初始参数
S_K1=zeros(mn);%一步预测状态值
S_K=zeros(mn); %滤波器输出值（一步滤波值经过修正之后）
K=zeros(mn);   %卡尔曼增益
M_K1=zeros(mn);%一步预测状态值协方差矩阵
M_K=zeros(mn); %滤波器输出值协方差矩阵

M_K(1)=1;
S_K(1)=23.3;

%kman核心算法
for n=2:over
    %时间更新
    S_K1(n)=S_K(n-1);   %一步预测
    M_K1(n)=M_K(n-1)+Q; %一步预测均方差矩阵
    %状态更新
    K(n)=M_K1(n)/(M_K1(n)+R);
    S_K(n)=S_K1(n)+K(n)*(X(n)-S_K1(n));
    M_K(n)=(1-K(n))*M_K1(n);
end

%绘图
LineWidth=2;

plot (X, 'k+');%画出温度计的测量值
hold on;

plot (S_K, 'b-')%画出最优估计值
hold off

参考文献：

[1] 赵树杰.赵建勋.信号检测于估计理论.清华大学出版社。

[2]

https://www.bilibili.com/video/BV1Rh41117MT?spm_id_from=333.337.search-card.all.clickhttps://www.bilibili.com/video/BV1Rh41117MT?spm_id_from=333.337.search-card.all.click[3]https://www.bilibili.com/video/BV1WZ4y1F7VN?spm_id_from=333.337.search-card.all.clickhttps://www.bilibili.com/video/BV1WZ4y1F7VN?spm_id_from=333.337.search-card.all.click

请叫我7plus

关注

25
点赞
踩
261

收藏

觉得还不错? 一键收藏
打赏
2
评论
卡尔曼滤波器原理讲解及其matlab实现

引言：在进行一些信号处理的过程中，我们通常会采集到一些数据，但是实际测量到的数据是受到噪声干扰了之后的，故与真实的数据有一些偏差。因此我们把 [ 通过测量数据进行一系列处理得到近似于真实数据 ] 的过程(或利用测量数据估计得到近似于真实数据的过程)称为 [ 估计 ] 。其中 [ 波形估计 ] 是很重要的一种工具，在目标跟踪，轨迹跟踪等方面具有很重要的应用。一般波形估计是通过一组数据得到另一组数据，就像是一组数据通过了一个滤波器，得到另一组数据。故经常会听到 [维纳滤波器]、[卡尔曼滤波器] 等词汇。
复制链接

扫一扫