【MATLAB】卡尔曼滤波器的原理及仿真(初学者专用)

文章目录

  • 0.引言
  • 1.场景预设
  • 2.卡尔曼滤波器
  • 3.仿真及效果

0.引言

\qquad 本文参考了Matlab对卡尔曼滤波器的官方教程及帮助文档(Kalman Filter)。官方教程的B站链接如下,在此对分享资源的Up主表示感谢。(如不能正常播放或需要看中文字幕,请点击此处B站链接)

【官方教程】卡尔曼滤波器教程与MATLAB仿真(全)(中英字幕)

另外提供友情链接如下:

  1. 卡尔曼滤波算法(知乎,多维情形)
  2. Matlab关于传感器融合知识的介绍
  3. 扩展卡尔曼滤波算法(CSDN,适合入门)
  4. 扩展卡尔曼滤波算法(知乎,适合入门)
  5. 扩展卡尔曼滤波算法(知乎,适合进阶)

\qquad本文不会完全照搬视频中的所有内容,只会介绍有关卡尔曼滤波器关于定位方面的知识。卡尔曼滤波器除最原始的版本(KF)外,其延伸版本主要有三种——扩展卡尔曼滤波器(EKF)、无迹卡尔曼滤波器(UKF)、粒子滤波器(PF)。它们的运算复杂度依次递增,其中KF、EKF、UKF是建立在随机噪声是高斯分布的基础上的;PF理论则没有此预设。它们的关系如下图所示:
不同卡尔曼滤波器比较

\qquad首先,KF是一种状态观测器。状态观测器是针对可观系统,根据输出yyy和输入uuu对系统内部状态uuu进行观测的结构单元。其构图如下(参考视频链接):
状态观测器
原系统方程为:
{x˙=Ax+Buy=Cx\begin{cases} \dot{x}=Ax+Bu \\ y=Cx \end{cases} {x˙=Ax+Buy=Cx
采用状态观测器的观测系统方程为:
{x^˙=Ax^+Bu+K(y−y^)y^=Cx^\begin{cases} \dot{\hat{x}}=A\hat{x}+Bu+K(y-\hat{y}) \\ \hat{y}=C\hat{x} \end{cases} {x^˙=Ax^+Bu+K(yy^)y^=Cx^
为保证观测器的lim⁡t→∞(x−x^)T(x−x^)=0\lim_{t\rightarrow\infty}(x-\hat{x})^T(x-\hat{x})=0tlim(xx^)T(xx^)=0
需要A−KCA-KCAKC是负定的。而卡尔曼滤波器就是一种状态观测器,只不过它是随机系统的状态观测器,其结构框图如下:
卡尔曼滤波器
在输入uuu和动态系统Plant中间会引入过程噪声www,而在输出yyy(即测量)和实际测量yvy_vyv中间会引入传感器噪声vvv,而卡尔曼滤波器则是根据u,yvu,y_vu,yv求得测量的最优估计yey_eye

1.场景预设

\qquad 假设一辆汽车在做匀速直线运动(一维场景),驾驶员通过油门控制了汽车的加速度恒定。忽略汽车所受的阻力、质量变化,并假设驾驶员操作会给汽车的牵引力造成一定的过程噪声。选择汽车的位移和速度为状态变量x=[p,v]Tx=[p,v]^Tx=[p,v]T,则状态变量导数为汽车的速度和加速度即x˙=[v,a]T\dot{x}=[v,a]^Tx˙=[v,a]T,选取控制变量u=au=au=a,测量量为汽车的位移。则状态方程如下:
{x˙=[0100]x+[01]uy=[01]x\begin{cases} \dot{x}=\begin{bmatrix} 0 &1\\0 & 0 \end{bmatrix}x+\begin{bmatrix} 0 \\ 1 \end{bmatrix}u\\[2ex] y=\begin{bmatrix} 0 & 1 \end{bmatrix}x \end{cases} x˙=[0010]x+[01]uy=[01]x
选取采用间隔dtdtdt,则状态方程离散化后变为:
{x(n)=[1dt01]x(n−1)+[0dt]u(n−1)y(n)=[01]x(n)\begin{cases} x(n)=\begin{bmatrix} 1 &dt \\ 0 & 1 \end{bmatrix}x(n-1)+\begin{bmatrix} 0 \\ dt \end{bmatrix}u(n-1)\\[2ex] y(n)=\begin{bmatrix} 0 & 1 \end{bmatrix}x(n) \end{cases} x(n)=[10dt1]x(n1)+[0dt]u(n1)y(n)=[01]x(n)
按照惯例定义
A=[1dt01],B=[0dt],C=[01],I=[1001]A=\begin{bmatrix} 1 &dt \\ 0 & 1 \end{bmatrix},B=\begin{bmatrix} 0 \\ dt \end{bmatrix},C=\begin{bmatrix} 0 & 1 \end{bmatrix}, I=\begin{bmatrix} 1 &0 \\ 0 & 1 \end{bmatrix} A=[10dt1],B=[0dt],C=[01],I=[1001]
由于系统具有一定的过程噪声www和测量噪声vvv。引入噪声的离散系统状态方程为:
{x(n)=Ax(n−1)+B(u(n−1)+w′)=Bu(n)+wyv(n)=Cx(n)+v\begin{cases} x(n)=Ax(n-1)+B(u(n-1)+w')=Bu(n)+w\\ y_v(n)=Cx(n)+v \end{cases} {x(n)=Ax(n1)+B(u(n1)+w)=Bu(n)+wyv(n)=Cx(n)+v
为加以区别,使用yvy_vyv在本文中表示含噪声的真实测量。定义随机噪声的方差:
E((w−w‾)T(w−w‾))=Q,E((v−v‾)T(v−v‾))=RE((w-\overline{w})^T(w-\overline{w}))=Q,E((v-\overline{v})^T(v-\overline{v}))=RE((ww)T(ww))=Q,E((vv)T(vv))=R
汽车的初始真实位移为0.2,真实速度为0即x0=[0.2,0]Tx_0=[0.2,0]^Tx0=[0.2,0]T.

2.卡尔曼滤波器

\qquad为方便初学者入门,本文不会从贝叶斯估计的角度证明KF的公式,但会将其应用步骤以尽可能简单的手段列出。

  1. 需要设定一个初始的状态估计值即x^0\hat{x}_0x^0,需要认为规定初始的先验估计的协方差矩阵P0=E((x−x^)T(x−x^))P_0=E((x-\hat{x})^T(x-\hat{x}))P0=E((xx^)T(xx^))x^0\hat{x}_0x^0一般是凭经验设定的,如果系统偏离平衡状态不远设定[0,0]T[0,0]^T[0,0]T也无妨;P0P_0P0在教学视频中并未给出设定方式,一般设定为QQQ即可。令k=1k=1k=1,仿真开始。
  2. 获取当前测量量yvy_vyv,先求出x,Px,Px,P的先验估计,即x^k−=Ax^k−1+BukPk−=APk−1AT+Q\hat{x}^{-}_k=A\hat{x}_{k-1}+Bu_k \\[2ex] P_k^-=AP_{k-1}A^T+Qx^k=Ax^k1+BukPk=APk1AT+Q
  3. 根据x,Px,Px,P的先验估计,求出xxx的后验估计x^\hat{x}x^,即Kk=Pk−CTCPk−CT+Rx^k=x^k−+Kk(yv−Cx^k−)Pk=(I−KkC)Pk−K_k=\frac{P_k^-C^T}{CP_k^-C^T+R}\\[2ex] \hat{x}_k=\hat{x}^-_k+K_k(y_v-C\hat{x}^-_k)\\[2ex] P_k=(I-K_kC)P_k^-Kk=CPkCT+RPkCTx^k=x^k+Kk(yvCx^k)Pk=(IKkC)Pk
  4. 根据y^=Cx^\hat{y}=C\hat{x}y^=Cx^求出测量的卡尔曼估计值y^\hat{y}y^.
  5. k=k+1k=k+1k=k+1,转步2

\qquad严格地来说,如果步骤4要成立,需要E((w−w‾)T(x−x‾))=0E((w-\overline{w})^T(x-\overline{x}))=0E((ww)T(xx))=0wwwvvv下相互独立,且系统方程中直接传输矩阵D=0D=0D=0,但考虑到本文是写给初学者的,所以在此默认了两个条件是成立的。
\qquad状态协方差即为P,测量协方差为CPCTCPC^TCPCT。卡尔曼滤波器算法收敛一般是指的是测量协方差收敛。

3.仿真及效果

在此附上Matlab的仿真代码

% 模拟要求汽车在一维空间的加速和减速过程
% 控制变量u是汽车的加速度
% 状态变量x=[p,v],x^hat=[v,a]
% w为控制变量的随机扰动,v为测量的随机扰动
% Q为w的方差,R为v的方差,假设w与v相互独立
clear
dt = 0.1;  % 采样间隔
N = 100;  % 仿真数
Q = diag([0,0.05]); R = 1;
A = [1,dt;0,1];
B = [0;dt];
C = [1,0];
P = Q;
I = eye(2);
x0 = [0.2;0];  % 初始位置和速度
xh0 = [0;0];  % x0的估计
u = ones(1,N);  % 加速度恒定
w = sqrt(Q)*randn(2,N); % 控制变量的误差2*N
v = sqrt(R)*randn(1,N); % 测量误差1*N
ye_list = zeros(size(u));  % 估计值
yv_list = zeros(size(u));  % 测量值
y_list = zeros(size(u));  % 实际值
cov_list = zeros(size(u));  % 测量方差
for i = 1:numel(u)xreal = A*x0 + B*u(i);  % 真实的状态变量yreal = C*x0;  % 真实的测量x1 = xreal + w(:,i);  % 含噪声的状态变量yv = yreal + v(i);  % 含噪声的测量xfe = A*xh0 + B*u(i);  % 先验的状态变量Pfe = A*P*A'+ Q;  % 先验的状态变量协方差K = Pfe*C'/(C*Pfe*C'+R);  % 卡尔曼最优增益xh1 = xfe + K*(yv-C*xfe);  % 当前的状态估计ye = C*xh1;P = (I-K*C)*Pfe;x0 = x1;xh0 = xh1;y_list(i) = yreal;yv_list(i) = yv;ye_list(i) = ye;cov_list(i) = C*P*C';
end
ax = (1:N).*dt;
figure(1);
subplot(2,2,1)
plot(ax,y_list,ax,yv_list,ax,ye_list)
legend('实际','测量','估计','Location','best')
title('汽车的位移')
ylabel('位移/m')
xlabel('时间/s')
subplot(2,2,2)
plot(ax,yv_list-y_list,ax,ye_list-y_list)
legend('测量','估计','Location','best')
title('汽车的位移误差')
ylabel('位移/m')
xlabel('时间/s')
subplot(2,2,[3,4])
plot(ax,cov_list)
legend('测量方差','Location','best')
title('测量方差')
ylabel('方差/m^2')
xlabel('时间/s')

本文设定的采样间隔dt=0.1,x0=[0.2,0]T,x^0=[0,0]Tdt=0.1,x_0=[0.2,0]^T,\hat{x}_0=[0,0]^Tdt=0.1x0=[0.2,0]T,x^0=[0,0]T,注意由于w=Bw′w=Bw'w=Bw,而B的第一行为0,故QQQ的对角线第一元素必定为0.仿真的效果图如下:
卡尔曼滤波器定位仿真

实际值即状态方程的输出y=Cxy=Cxy=Cx,测量值即含噪声的输出yv=Cx+vy_v=Cx+vyv=Cx+v,估计值为卡尔曼滤波器对测量的最优估计值。
\qquad希望本文对您有所帮助,谢谢阅读!如有异议,欢迎评论区指正!

另外对本话题感兴趣的读者可以继续阅读有关适用于非线性系统的扩展卡尔曼滤波算法的介绍。

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/544447.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

Go实现查找目录下(包括子目录)替换文件内容

为什么80%的码农都做不了架构师?>>> 【功能】 按指定的目录查找出文件,如果有子目录,子目录也将进行搜索,将其中的文件内容进行替换。 【缺陷】 1. 没有过滤出文本文件 2. 当文件过大时,效率不高 【代码】…

卡诺模板_无关条件的卡诺地图

卡诺模板Till now, the Boolean expressions which have been discussed by us were completely specified, i.e., for each combination of input variable we have specified a minterm by representing them as 1 in the K-Map. But, there may arise a case when for a giv…

面试官:final、finally、finalize 有什么区别?

作者 | 磊哥来源 | Java面试真题解析(ID:aimianshi666)转载请联系授权(微信ID:GG_Stone)重要说明:本篇为博主《面试题精选-基础篇》系列中的一篇,查看系列面试文章请关注我。Gitee 开…

【Matlab】扩展卡尔曼滤波器原理及仿真(初学者入门专用)

文章目录0.引言及友情链接1.场景预设2.扩展卡尔曼滤波器3.仿真及效果0.引言及友情链接 \qquad卡尔曼滤波器(Kalman Filter, KF)是传感器融合(Sensor Fusion)的基础,虽然知乎、CSDN、GitHub等平台已有大量的学习资料&am…

Windows 8.1 升级到专业版

本例将一台 Windows 8.1 平板升级到专业版。升级前:升级的原因,是因为用户发现这台平板不能启用远程桌面管理。查看计算机属性,显示如下:从上面的信息可以看出,目前这台平板安装的不是专业版。具体是什么版本呢&#x…

【MATLAB】求点到多边形的最短距离

文章目录0.引言1.原理2.代码及实用教程0.引言 \qquad点与多边形的关系无非三种——内部、上、外部。本文定义点在多边形内部距离为负,点在多边形边上距离为0,到多边形外部距离为正。 1.原理 计算点到多边形的距离分为3个步骤: 判断点与多边…

绝了,66道并发多线程面试题汇总

👆🏻一个专注于 Java 面试的原创公众号。我花了点时间整理了一些多线程,并发相关的面试题,虽然不是很多,但是偶尔看看还是很有用的哦!话不多说,直接开整!01 什么是线程?线程是操作系…

ruby array_Array.select! Ruby中的示例方法

ruby arrayArray.select! 方法 (Array.select! Method) In this article, we will study about Array.select! Method. You all must be thinking the method must be doing something related to the selection of objects from the Array instance. It is not as …

【Python】mmSegmentation语义分割框架教程(自定义数据集、训练设定、数据增强)

文章目录0.mmSegmentation介绍1.mmSegmentation基本框架1.1.mmSegmentation的model设置1.2.mmSegmentation的dataset设置1.2.1.Dataset Class文件配置1.2.2.Dataset Config文件配置1.2.3.Total Config文件配置2.运行代码 3.展示效果图和预测X.附录X.1.mmSegmentation框架解释 X…

基本shell编程【3】- 常用的工具awk\sed\sort\uniq\od

awk awk是个很好用的东西,大量使用在linux系统分析的结果展示处理上。并且可以使用管道, input | awk | output1.首先要知道形式awk command file 如 awk {print $0} a.txt b.txt (后面可以跟一个或多个文件)2.command学习。c…

面试官:重写 equals 时为什么一定要重写 hashCode?

作者 | 磊哥来源 | Java面试真题解析(ID:aimianshi666)转载请联系授权(微信ID:GG_Stone)重要说明:本篇为博主《面试题精选-基础篇》系列中的一篇,关注我,查看更多面试题。…

rotate array_Array.rotate! Ruby中的示例方法

rotate arrayArray.rotate! 方法 (Array.rotate! Method) In this article, we will study about Array.rotate! method. You all must be thinking the method must be doing something which is related to rotating certain elements. It is not as simple as it…

【python】获取PC机公网IP并发送至邮箱

文章目录0.引言1.获取外网IP2.打开SMTP服务3.python发送邮件4.完整代码0.引言 \qquad之前一直使用Putty连接公司的PC机进行远程办公,苦于外网的IP地址不能固定下来,所以购买了内网穿透服务,免费版还会限速。后来转念一想,如果能定…

List 去重的 6 种方法,这个方法最完美!

作者 | 王磊来源 | Java中文社群(ID:javacn666)转载请联系授权(微信ID:GG_Stone)在日常的业务开发中,偶尔会遇到需要将 List 集合中的重复数据去除掉的场景。这个时候可能有同学会问&#xff1a…

Mongodb -(3) replica set+sharding

分片集搭建---何旭东目录分片集搭建...................................................................................................................... 1生态系统...............................................................................................…

electron 菜单栏_如何在Electron JS中添加任务栏图标菜单?

electron 菜单栏If you are new here, please consider checking out my recent articles on Electron JS including Tray Icons. 如果您是新来的,请考虑查看我最近关于Electron JS的文章, 包括托盘图标 。 In this tutorial, we will set up 2 menu it…

【逆强化学习-0】Introduction

文章目录专栏传送门0.引言1.逆强化学习发展历程2.需要准备的专栏传送门 0.简介 1.学徒学习 2.最大熵学习 0.引言 \qquad相比于深度学习,国内强化学习的教程并不是特别多,而相比强化学习,逆强化学习的教程可谓是少之又少。而本人想将整理到的资…

不知道Mysql排序的特性,加班到12点,认了认了!

小弟新写了一个功能,自测和测试环境测试都没问题,但在生产环境会出现偶发问题。于是,加班到12点一直排查问题,终于定位了的问题原因:Mysql Limit查询优化导致。现抽象出问题模型及解决方案,分析给大家&…

js中==与===的区别

2019独角兽企业重金招聘Python工程师标准>>> 1、对于string,number等基础类型,和是有区别的 1)不同类型间比较,之比较“转化成同一类型后的值”看“值”是否相等,如果类型不同,其结果就是不等 2&#xff09…

c语言中memcpy函数_带有示例的C中的memcpy()函数

c语言中memcpy函数memcpy()函数 (memcpy() function) memcpy() is a library function, which is declared in the “string.h” header file - it is used to copy a block of memory from one location to another (it can also be considered as to copy a string to anothe…