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

文章目录

  • 0.引言及友情链接
  • 1.场景预设
  • 2.扩展卡尔曼滤波器
  • 3.仿真及效果

0.引言及友情链接

\qquad卡尔曼滤波器(Kalman Filter, KF)是传感器融合(Sensor Fusion)的基础,虽然知乎、CSDN、GitHub等平台已有大量的学习资料,但还是建议大家在看完B站Matlab Tech Talk有关卡尔曼滤波器视频后再进入深入学习。友情链接提供如下:

  • B站Matlab官方视频
  • 卡尔曼滤波器介绍(CSDN,初学者专用)
  • 卡尔曼滤波器介绍(知乎,进阶篇)
  • 扩展卡尔曼滤波器原理(知乎,适合入门)
  • 扩展卡尔曼与无迹卡尔曼(知乎,进阶篇)
  • 扩展卡尔曼滤波器公式推导(Github.io)

\qquad在此感谢各位辛勤的知乎、CSDN作者及B站分享视频的Up主。未学习卡尔曼滤波器的读者可学习链接中的1和2,本文将介绍扩展卡尔曼滤波器(Extended Kalman Filter, EKF)的原理并以一个有关汽车运动的Matlab仿真说明其应用。
\qquad看过我上一篇介绍KF的博客的读者肯定知道KF设计的目的和结构,即让状态估计的方差随时间推移趋于0,然而由于KF针对的是随机系统,这一点往往做不到,而只能使其收敛为一个常数。EKF和KF的结构差不多,只不过EKF针对的是非线性系统的滤波器。不含随机噪声的非线性系统状态方程如下:
Nonlinear System:
{x(k)=f(x(k−1),u(k−1))y(k)=g(x(k),u(k))\begin{cases} x(k)=f(x(k-1),u(k-1))\\ y(k)=g(x(k),u(k))\\ \end{cases} {x(k)=f(x(k1),u(k1))y(k)=g(x(k),u(k))
引入EKF后,其结构框图如下:
扩展卡尔曼滤波器框图
\qquad其中x^\hat{x}x^为估计状态,www为过程噪声(一般由控制变量uuu引入,但也可能由物理系统本身的不确定性引入),而vvv为测量噪声。根据Matlab的帮助文档介绍,噪声也分为两种——加入型噪声(Additive Noise)和非加入型噪声(Nonadditive Noise),其分类取决于噪声是否在非线性函数内部,二者的状态方程形式如下:
Additive Noise:
{x(k)=f(x(k−1),u(k))+w(k)y(k)=g(x(k),u(k))+v(k)\begin{cases} x(k)=f(x(k-1),u(k))+w(k)\\ y(k)=g(x(k),u(k))+v(k)\\ \end{cases} {x(k)=f(x(k1),u(k))+w(k)y(k)=g(x(k),u(k))+v(k)
Nonadditive Noise
{x(k)=f(x(k−1),u(k),w(k))y(k)=g(x(k),u(k),v(k))\begin{cases} x(k)=f(x(k-1),u(k),w(k))\\ y(k)=g(x(k),u(k),v(k))\\ \end{cases} {x(k)=f(x(k1),u(k),w(k))y(k)=g(x(k),u(k),v(k))
从上述表达式也可以看出加入型噪声是非加入型的特例。

1.场景预设

\qquad为了应用EKF,需要构造一个非线性系统,与前一篇讲述KF的博文保持连续性。这次使用的仍然所示一维的汽车运动模型,状态变量仍然选择汽车的位移和速度(x=[p,v]T)(x=[p,v]^T)(x=[p,v]T),但这次控制变量为u(k)u(k)u(k)为汽车的功率/汽车的质量,重新构造状态方程如下:
{x˙=f(x,u)=A0∗x+B0∗Tu/(B0∗x)y=g(x)=12(C0∗x)T(C0∗x)\begin{cases} \dot{x}=f(x,u)=A_0^* x+B_0^{*T}u/(B_0^*x)\\ y=g(x)=\frac{1}{2}(C_0^*x)^T(C_0^*x) \end{cases} {x˙=f(x,u)=A0x+B0Tu/(B0x)y=g(x)=21(C0x)T(C0x)
其中A0∗=[0100],B0∗=C0∗=[01]A_0^*=\begin{bmatrix} 0 &1\\0 & 0 \end{bmatrix},B_0^*=C_0^*=\begin{bmatrix}0 &1\end{bmatrix}A0=[0010],B0=C0=[01]
为了与控制变量uuu保持一致性,此处的测量yyy为单位质量产生的动能。为了不失一般性,添加非加入型噪声如下(同样以yvy_vyv表示测量值,yyy表示实际值,y^\hat{y}y^表示估计值):
{x˙=f(x,u)=A0∗x+B0∗T(u+w)/(B0∗x)yv=g(x)=12(C0∗x)T(C0∗x)+v\begin{cases} \dot{x}=f(x,u)=A_0^* x+B_0^{*T}(u+w)/(B_0^*x)\\ y_v=g(x)=\frac{1}{2}(C_0^*x)^T(C_0^*x)+v \end{cases} {x˙=f(x,u)=A0x+B0T(u+w)/(B0x)yv=g(x)=21(C0x)T(C0x)+v
设定采样时间dtdtdt,状态方程化为离散形式:
{x(n)=f(x(n−1),u(n−1))=A0+B0T(u(n−1)+w)/(B0x(n−1))yv(n)=g(x(n))=12(C0x(n))T(C0x(n))+v\begin{cases} x(n)=f(x(n-1),u(n-1))=A_0+B_0^T(u(n-1)+w)/(B_0x(n-1))\\ y_v(n)=g(x(n))=\frac{1}{2}(C_0x(n))^T(C_0x(n))+v \end{cases} {x(n)=f(x(n1),u(n1))=A0+B0T(u(n1)+w)/(B0x(n1))yv(n)=g(x(n))=21(C0x(n))T(C0x(n))+v
其中A0=[1dt01],B0=[01]A_0=\begin{bmatrix} 1 &dt\\0 & 1 \end{bmatrix},B_0=\begin{bmatrix}0 &1\end{bmatrix}A0=[10dt1],B0=[01]
与上一篇博文不同,设E((B0w−B0w‾)T(B0w−B0w‾))=Q,E((v−v‾)T(v−v‾))=RE((B_0w-\overline{B_0w})^T(B_0w-\overline{B_0w}))=Q,E((v-\overline{v})^T(v-\overline{v}))=RE((B0wB0w)T(B0wB0w))=Q,E((vv)T(vv))=R

2.扩展卡尔曼滤波器

\qquad与上一篇博文一样,本文不会从概率论或者最优化理论的角度对EKF的公式加以深入推导,但会详细列出EKF最优估计的算法步骤。步骤会与Matlab的帮助文档的计算顺序略有出入,但经过实验比较之后结果几乎没有差异。

  1. 设定初始状态变量的估计值x^0\hat{x}_0x^0,并计算以下导数及P的初始值:A0=∂f∂x∣x^0,u0G0=∂f∂w∣x^0,u0C0=∂g∂x∣x^0S0=∂g∂v∣x^0P0=G0QG0TA_0= \frac{\partial f}{\partial x}|_{\hat{x}_0,u_0}\\[2ex]G_0=\frac{\partial f}{\partial w}|_{\hat{x}_0,u_0}\\[2ex]C_0=\frac{\partial g}{\partial x}|_{\hat{x}_0}\\[2ex]S_0=\frac{\partial g}{\partial v}|_{\hat{x}_0}\\[2ex]P_0=G_0QG_0^TA0=xfx^0,u0G0=wfx^0,u0C0=xgx^0S0=vgx^0P0=G0QG0Tk=1k=1k=1
  2. 获取当前测量量yvy_vyv,计算状态变量的先验估计x^k−=f(x^k−1,uk−1)\hat{x}_k^-=f(\hat{x}_{k-1},u_{k-1})x^k=f(x^k1,uk1)
  3. 计算以下导数:Ak=∂f∂x∣x^k−,ukGk=∂f∂w∣x^k−,ukCk=∂g∂x∣x^k−Sk=∂g∂v∣x^k−A_k= \frac{\partial f}{\partial x}|_{\hat{x}_k^-,u_k}\\[2ex]G_k=\frac{\partial f}{\partial w}|_{\hat{x}_k^-,u_k}\\[2ex]C_k=\frac{\partial g}{\partial x}|_{\hat{x}_k^-}\\[2ex]S_k=\frac{\partial g}{\partial v}|_{\hat{x}_k^-}\\[2ex]Ak=xfx^k,ukGk=wfx^k,ukCk=xgx^kSk=vgx^k并顺带计算Pk−=AkPAkT+GkQGkTP_k^-=A_kPA_k^T+G_kQG_k^TPk=AkPAkT+GkQGkT
  4. 计算EKF的最优增益Kk=Pk−CkT(CkPk−CkT+SkRSkT)−1K_k=P_k^-C_k^T(C_kP_k^-C_k^T+S_kRS_k^T)^{-1}Kk=PkCkT(CkPkCkT+SkRSkT)1
  5. 更新Pk=(I−KkCk)Pk−P_k=(I-K_kC_k)P_k^-Pk=(IKkCk)Pk并计算状态变量的后验估计x^k=x^k−+Kk(yv−g(xk−))\hat{x}_k=\hat{x}^-_k+K_k(y_v-g(x^-_k))x^k=x^k+Kk(yvg(xk))
  6. 计算测量量的估计值y^k=g(x^k)\hat{y}_k=g(\hat{x}_k)y^k=g(x^k),令k=k+1k=k+1k=k+1,转步2

3.仿真及效果

仿真的Matlab代码如下:

% 模拟要求汽车在一维空间的加速和减速过程
% 控制变量u是汽车的加速度
% 状态变量x=[p,v],x^hat=[v,a]
% w为控制变量的随机扰动,v为测量的随机扰动
% Q为w的方差,R为v的方差,假设w与v相互独立
clear
dt = 0.1;  % 采样间隔
m = 1e3;  % 汽车自重
N = 100;  % 仿真数
Q = 2e-4; % 过程噪声的协方差矩阵
R = 0.01;  % 测量噪声的方差
x0 = [0;0.5];  % 初始位置和速度
xh0 = [1;0.4];  % x0的估计
A0 = [1,dt;0,1];
B0 = [0,1];
f = @(x,u)(A0*x+B0'*dt*u./(B0*x));  % 系统方程的非线性函数
C0 = sqrt([0,10]);
g = @(x,v)(1/2*(C0*x)'*(C0*x)+v);  % 输出方程的非线性函数
A = @(x,u)(A0+[0,0;0,-dt*u/x(2)^2]);  % pf/px 2*2
G = @(x)([0;-dt/x(2)]);  % pf/pw 2*1
C = @(x)(C0.*x');  % pg/px 1*2
S = 1;  % pg/pv 2*1
P = G(xh0)*Q*G(xh0)';  % 2*2
I = eye(2);
u = 0.01*ones(1,N);  % 功率恒定 1*1
w = sqrt(Q)*randn(1,N); % 控制变量的误差1*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 = f(x0,u(i));  % 真实的状态变量yreal = g(x0,0);  % 真实的测量x1 = f(x0,u(i)+w(i));  % 含噪声的状态变量 2*1yv = g(x0,v(i));  % 含噪声的测量 1*1xfe = f(xh0,u(i));Pfe = A(xfe,u(i))*P*A(xfe,u(i))'+G(xfe)*Q*G(xfe)';K = Pfe*C(xfe)'/(C(xfe)*Pfe*C(xfe)'+S*R*S');  % 卡尔曼最优增益 2*1P = (I - K*C(xfe))*Pfe;  % 当前状态先验估计协方差xh1 = xfe+K*(yv-g(xfe,0));  % 状态预测ye = g(xh1,0);x0 = x1;xh0 = xh1;y_list(i) = yreal;yv_list(i) = yv;ye_list(i) = ye;cov_list(i) = C(xh1)*P*C(xh1)';
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('动能/J')
xlabel('时间/s')
subplot(2,2,2)
plot(ax,yv_list-y_list,ax,ye_list-y_list)
legend('测量','估计','Location','best')
title('汽车的动能误差')
ylabel('动能/J')
xlabel('时间/s')
subplot(2,2,[3,4])
plot(ax,cov_list)
legend('测量方差','Location','best')
title('测量方差')
ylabel('方差/m^2')
xlabel('时间/s')

\qquad这里设定的汽车的功率/质量为恒定的0.01,汽车初速度为1m/s,为恒功率加速过程,根据能量守恒定律,其测量量(单位质量的动能)应成近似线性增长。相关效果图如下:
扩展卡尔曼滤波器仿真

\qquad通过效果图可以看出,虽然初始状态时估计值x^0\hat{x}_0x^0与真实值x0x_0x0相差较大造成EKF的误差较测量值较大,但是经过一段时间推移后,EKF的测量误差逐渐减小并较测量值有了显著提升。EKF算法对于非线性系统是近似收敛的,从测量方差的走势也可以看出。需要指出的是这里的测量方差是单位质量的动能,真实的动能需要乘上汽车的质量。
\qquad希望本文对您有帮助,谢谢阅读。若有异议,欢迎评论区指正!

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

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

相关文章

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…

【逆强化学习-1】学徒学习(Apprenticeship Learning)

文章目录0.引言1.算法原理2.仿真环境3.运行4.补充(学徒学习深度Q网络)本文为逆强化学习系列第1篇,没有看过逆强化学习介绍的那篇的朋友,可以看一下:Inverse Reinforcement Learning-Introduction 传送门 0.引言 \qquad…

面试官:HashMap有几种遍历方法?推荐使用哪种?

作者 | 磊哥来源 | Java面试真题解析(ID:aimianshi666)转载请联系授权(微信ID:GG_Stone)HashMap 的遍历方法有很多种,不同的 JDK 版本有不同的写法,其中 JDK 8 就提供了 3 种 HashMa…

HTML 5 input placeholder 属性

<input placeholder"请先选择组织" type"text" value"" </input>placeholder 属性提供可描述输入字段预期值的提示信息&#xff08;hint&#xff09;。 该提示会在输入字段为空时显示&#xff0c;并会在字段获得焦点时消失。 注释&…

【逆强化学习-2】最大熵学习(Maximum Entropy Learning)

文章目录0.引言1.算法原理2.仿真0.引言 \qquad本文是逆强化学习系列的第2篇&#xff0c;其余博客传送门如下&#xff1a; 逆强化学习0-Introduction 逆强化学习1-学徒学习 \qquad最大熵学习是2008年出现的方法&#xff0c;原论文&#xff08;链接见【逆强化学习0】的博客&#…