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(k−1),u(k−1))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(k−1),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(k−1),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)=A0∗x+B0∗Tu/(B0∗x)y=g(x)=21(C0∗x)T(C0∗x)
其中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)=A0∗x+B0∗T(u+w)/(B0∗x)yv=g(x)=21(C0∗x)T(C0∗x)+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(n−1),u(n−1))=A0+B0T(u(n−1)+w)/(B0x(n−1))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((B0w−B0w)T(B0w−B0w))=Q,E((v−v)T(v−v))=R

2.扩展卡尔曼滤波器

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

设定初始状态变量的估计值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=∂x∂f∣x^0,u0G0=∂w∂f∣x^0,u0C0=∂x∂g∣x^0S0=∂v∂g∣x^0P0=G0QG0T令k=1k=1k=1
获取当前测量量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^k−1,uk−1)
计算以下导数：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=∂x∂f∣x^k−,ukGk=∂w∂f∣x^k−,ukCk=∂x∂g∣x^k−Sk=∂v∂g∣x^k−并顺带计算Pk−=AkPAkT+GkQGkTP_k^-=A_kPA_k^T+G_kQG_k^TPk−=AkPAkT+GkQGkT
计算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=Pk−CkT(CkPk−CkT+SkRSkT)−1
更新Pk=(I−KkCk)Pk−P_k=(I-K_kC_k)P_k^-Pk=(I−KkCk)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(yv−g(xk−))
计算测量量的估计值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希望本文对您有帮助，谢谢阅读。若有异议，欢迎评论区指正！

【Matlab】扩展卡尔曼滤波器原理及仿真（初学者入门专用）相关推荐

【目标定位】基于matlab扩展卡尔曼滤波器多机器人定位【含Matlab源码 2327期】
⛄一.简介 1 机器人运动模型分析 1.2 坐标系一个是全局坐标系(Xw,Yw), 另一个是机器人的局部坐标系(Xr,Yr). 如下图所示: 图1 机器人坐标系室内环境用二维平面表示在全局坐标系X ...
【MATLAB】卡尔曼滤波器的原理及仿真（初学者专用）
文章目录 0.引言 1.场景预设 2.卡尔曼滤波器 3.仿真及效果 0.引言 \qquad 本文参考了Matlab对卡尔曼滤波器的官方教程及帮助文档(Kalman Filter).官方教程的B站链接如 ...
【信号处理】基于扩展卡尔曼滤波器和无迹卡尔曼滤波器的窄带信号时变频率估计（Matlab代码实现）
目录 1 概述 2 数学模型 3 运行结果 4 结论 5 参考文献 6 Matlab代码实现 1 概述本文讲解和比较了基于卡尔曼滤波器的频率跟踪方法的能力,例如扩展卡尔曼滤波器 (EKF) 和无味卡 ...
MATLAB实现卡尔曼滤波器仿真
MATLAB实现卡尔曼滤波器仿真本仿真是在学习B站DR_CAN的视频之后的练习.联系针对的是第五个视频中的二维示例. 视频连接为 [[卡尔曼滤波器]1_递归算法_Recursive Processi ...
基于自适应扩展卡尔曼滤波器（AEKF）的锂离子电池SOC估计（附MATLAB代码）
AEKF_SOC_Estimation函数使用二阶RC等效电路模型(ECM)和自适应扩展卡尔曼滤波器(AEKF)估计电池的端电压(Vt)和充电状态(SOC).该函数将以下内容作为输入: · 电流(A) ...
说不尽的卡尔曼 | 详解扩展卡尔曼滤波器
作者 | 火山编辑 | 空中机器人前沿点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取点击进入→自动驾驶之心[目标跟踪]技术交流群后台回复[卡尔曼滤波] ...
由浅入深的扩展卡尔曼滤波器教程
本篇译文翻译自 The Extended Kalman Filter : An Interactive Turorial for Non-Experts. 原文本文惯例及说明 : 译文中的Demo请 ...
卡尔曼转矩观测_基于扩展卡尔曼滤波器的表贴式永磁同步电机负载转矩观测方法与流程...
本发明涉及永磁同步电机技术领域,具体涉及基于扩展卡尔曼滤波器的表贴式永磁同步电机负载转矩观测方法. 背景技术: 永磁同步电机(pmsm:permanentmagnetsynchronousmotor) ...
有源电力滤波器matlab仿真实验报告,基于Matlab有源电力滤波器APF的仿真研究
目前,对有源电力滤波器的研究越来越广泛. 一方面,研究者众多,不仅有高等院校.研究所,而且也有许多电力局.大型企业等:另一方面,研究涉及谐波检测方法.控制策略.PWM 波的形成等有源滤波技术的各个方面 ...

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

文章目录

0.引言及友情链接

1.场景预设

2.扩展卡尔曼滤波器

3.仿真及效果

【Matlab】扩展卡尔曼滤波器原理及仿真（初学者入门专用）相关推荐

最新文章

热门文章