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

请叫我7plus 2023-11-06 原文

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

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

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

一：卡尔曼滤波器的信号模型[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

有关卡尔曼滤波器原理讲解及其matlab实现的更多相关文章

ruby - 如何根据特征实现 FactoryGirl 的条件行为 - 2
我有一个用户工厂。我希望默认情况下确认用户。但是鉴于unconfirmed特征，我不希望它们被确认。虽然我有一个基于实现细节而不是抽象的工作实现，但我想知道如何正确地做到这一点。factory:userdoafter(:create)do|user,evaluator|#unwantedimplementationdetailshereunlessFactoryGirl.factories[:user].defined_traits.map(&:name).include?(:unconfirmed)user.confirm!endendtrait:unconfirmeddoenden
世界前沿3D开发引擎HOOPS全面讲解——集3D数据读取、3D图形渲染、3D数据发布于一体的全新3D应用开发工具 - 2
无论您是想搭建桌面端、WEB端或者移动端APP应用，HOOPSPlatform组件都可以为您提供弹性的3D集成架构，同时，由工业领域3D技术专家组成的HOOPS技术团队也能为您提供技术支持服务。如果您的客户期望有一种在多个平台（桌面/WEB/APP，而且某些客户端是“瘦”客户端）快速、方便地将数据接入到3D应用系统的解决方案，并且当访问数据时，在各个平台上的性能和用户体验保持一致，HOOPSPlatform将帮助您完成。利用HOOPSPlatform，您可以开发在任何环境下的3D基础应用架构。HOOPSPlatform可以帮您打造3D创新型产品，HOOPSSDK包含的技术有：快速且准确的CAD
Matlab imread（）读到了什么（浅显当复习文档了） - 2
matlab打开matlab，用最简单的imread方法读取一个图像clcclearimg_h=imread('hua.jpg');返回一个数组（矩阵），往往是a*b*cunit8类型解释一下这个三维数组的意思，行数、数和层数，unit8：指数据类型，无符号八位整形，可理解为0~2^8的数三个层数分别代表RGB三个通道图像rgb最常用的是24-位实现方法，即RGB每个通道有256色阶（2^8）。基于这样的24-位RGB模型的色彩空间可以表现256×256×256≈1670万色当imshow传入了一个二维数组，它将以灰度方式绘制；可以把图像拆分为rgb三层，可以以灰度的方式观察它figure(1
华为OD机试用Python实现 -【明明的随机数】 2023Q1A - 2
华为OD机试题本篇题目：明明的随机数题目输入描述输出描述：示例1输入输出说明代码编写思路最近更新的博客华为od2023|什么是华为od，od薪资待遇，od机试题清单华为OD机试真题大全，用Python解华为机试题|机试宝典【华为OD机试】全流程解析+经验分享,题型分享,防作弊指南华为o
基于C#实现简易绘图工具【100010177】 - 2
C#实现简易绘图工具一.引言实验目的:通过制作窗体应用程序(C#画图软件),熟悉基本的窗体设计过程以及控件设计,事件处理等,熟悉使用C#的winform窗体进行绘图的基本步骤,对于面向对象编程有更加深刻的体会.Tutorial任务设计一个具有基本功能的画图软件**·包括简单的新建文件,保存,重新绘图等功能**·实现一些基本图形的绘制,包括铅笔和基本形状等,学习橡皮工具的创建**·设计一个合理舒适的UI界面**注明:你可能需要先了解一些关于winform窗体应用程序绘图的基本知识,以及关于GDI+类和结构的知识二.实验环境Windows系统下的visualstudio2017C#窗体应用程序三.
LC滤波器设计学习笔记（一）滤波电路入门 - 2
目录前言滤波电路科普主要分类实际情况单位的概念常用评价参数函数型滤波器简单分析滤波电路构成低通滤波器RC低通滤波器RL低通滤波器高通滤波器RC高通滤波器RL高通滤波器部分摘自《LC滤波器设计与制作》，侵权删。前言最近需要学习放大电路和滤波电路，但是由于只在之前做音乐频谱分析仪的时候简单了解过一点点运放，所以也是相当从零开始学习了。滤波电路科普主要分类滤波器：主要是从不同频率的成分中提取出特定频率的信号。有源滤波器：由RC元件与运算放大器组成的滤波器。可滤除某一次或多次谐波，最普通易于采用的无源滤波器结构是将电感与电容串联，可对主要次谐波（3、5、7）构成低阻抗旁路。无源滤波器：无源滤波器，又称
MIMO-OFDM无线通信技术及MATLAB实现（1）无线信道：传播和衰落 - 2
MIMO技术的优缺点优点通过下面三个增益来总体概括：阵列增益。阵列增益是指由于接收机通过对接收信号的相干合并而活得的平均SNR的提高。在发射机不知道信道信息的情况下，MIMO系统可以获得的阵列增益与接收天线数成正比复用增益。在采用空间复用方案的MIMO系统中，可以获得复用增益，即信道容量成倍增加。信道容量的增加与min(Nt,Nr)成正比分集增益。在采用空间分集方案的MIMO系统中，可以获得分集增益，即可靠性性能的改善。分集增益用独立衰落支路数来描述，即分集指数。在使用了空时编码的MIMO系统中，由于接收天线或发射天线之间的间距较远，可认为它们各自的大尺度衰落是相互独立的，因此分布式MIMO
【Java入门】使用Java实现文件夹的遍历 - 2
遍历文件夹我们通常是使用递归进行操作，这种方式比较简单，也比较容易理解。本文为大家介绍另一种不使用递归的方式，由于没有使用递归，只用到了循环和集合，所以效率更高一些！一、使用递归遍历文件夹整体思路1、使用File封装初始目录，2、打印这个目录3、获取这个目录下所有的子文件和子目录的数组。4、遍历这个数组，取出每个File对象4-1、如果File是否是一个文件，打印4-2、否则就是一个目录，递归调用代码实现publicclassSearchFile{publicstaticvoidmain(String[]args){//初始目录Filedir=newFile("d:/Dev");Datebeg
ruby - Arrays Sets 和 SortedSets 在 Ruby 中是如何实现的 - 2
通常，数组被实现为内存块，集合被实现为HashMap，有序集合被实现为跳跃列表。在Ruby中也是如此吗？我正在尝试从性能和内存占用方面评估Ruby中不同容器的使用情况最佳答案数组是Ruby核心库的一部分。每个Ruby实现都有自己的数组实现。Ruby语言规范只规定了Ruby数组的行为，并没有规定任何特定的实现策略。它甚至没有指定任何会强制或至少建议特定实现策略的性能约束。然而，大多数Rubyist对数组的性能特征有一些期望，这会迫使不符合它们的实现变得默默无闻，因为实际上没有人会使用它:插入、前置或追加以及删除元素的最坏情况步骤复
ruby - "public/protected/private"方法是如何实现的，我该如何模拟它？ - 2
在ruby中，你可以这样做:classThingpublicdeff1puts"f1"endprivatedeff2puts"f2"endpublicdeff3puts"f3"endprivatedeff4puts"f4"endend现在f1和f3是公共(public)的，f2和f4是私有(private)的。内部发生了什么，允许您调用一个类方法，然后更改方法定义？我怎样才能实现相同的功能(表面上是创建我自己的java之类的注释)例如...classThingfundeff1puts"hey"endnotfundeff2puts"hey"endendfun和notfun将更改以下函数定