【Matlab】扩展卡尔曼滤波器原理及仿真（初学者入门专用） |
您所在的位置：网站首页 › 卡尔曼滤波应用案例分析 › 【Matlab】扩展卡尔曼滤波器原理及仿真（初学者入门专用） |
【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(k−1),u(k−1))y(k)=g(x(k),u(k)) 引入EKF后，其结构框图如下： \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 ) = A 0 ∗ x + B 0 ∗ T u / ( B 0 ∗ x ) y = g ( x ) = 1 2 ( C 0 ∗ x ) T ( C 0 ∗ 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) 其中 A 0 ∗ = [ 0 1 0 0 ] , B 0 ∗ = C 0 ∗ = [ 0 1 ] 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] 为了与控制变量 u u u保持一致性，此处的测量 y y y为单位质量产生的动能。为了不失一般性，添加非加入型噪声如下（同样以 y v y_v yv表示测量值， y y y表示实际值， y ^ \hat{y} y^表示估计值）： { x ˙ = f ( x , u ) = A 0 ∗ x + B 0 ∗ T ( u + w ) / ( B 0 ∗ x ) y v = g ( x ) = 1 2 ( C 0 ∗ x ) T ( C 0 ∗ 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 设定采样时间 d t dt dt，状态方程化为离散形式： { x ( n ) = f ( x ( n − 1 ) , u ( n − 1 ) ) = A 0 + B 0 T ( u ( n − 1 ) + w ) / ( B 0 x ( n − 1 ) ) y v ( n ) = g ( x ( n ) ) = 1 2 ( C 0 x ( n ) ) T ( C 0 x ( 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 其中 A 0 = [ 1 d t 0 1 ] , B 0 = [ 0 1 ] A_0=\begin{bmatrix} 1 &dt\\0 & 1 \end{bmatrix},B_0=\begin{bmatrix}0 &1\end{bmatrix} A0=[10dt1],B0=[01] 与上一篇博文不同，设 E ( ( B 0 w − B 0 w ‾ ) T ( B 0 w − B 0 w ‾ ) ) = Q , E ( ( v − v ‾ ) T ( v − v ‾ ) ) = R E((B_0w-\overline{B_0w})^T(B_0w-\overline{B_0w}))=Q,E((v-\overline{v})^T(v-\overline{v}))=R E((B0w−B0w)T(B0w−B0w))=Q,E((v−v)T(v−v))=R 2.扩展卡尔曼滤波器\qquad 与上一篇博文一样，本文不会从概率论或者最优化理论的角度对EKF的公式加以深入推导，但会详细列出EKF最优估计的算法步骤。步骤会与Matlab的帮助文档的计算顺序略有出入，但经过实验比较之后结果几乎没有差异。 设定初始状态变量的估计值 x ^ 0 \hat{x}_0 x^0，并计算以下导数及P的初始值： A 0 = ∂ f ∂ x ∣ x ^ 0 , u 0 G 0 = ∂ f ∂ w ∣ x ^ 0 , u 0 C 0 = ∂ g ∂ x ∣ x ^ 0 S 0 = ∂ g ∂ v ∣ x ^ 0 P 0 = G 0 Q G 0 T A_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^T A0=∂x∂f∣x^0,u0G0=∂w∂f∣x^0,u0C0=∂x∂g∣x^0S0=∂v∂g∣x^0P0=G0QG0T令 k = 1 k=1 k=1获取当前测量量 y v y_v yv，计算状态变量的先验估计 x ^ k − = f ( x ^ k − 1 , u k − 1 ) \hat{x}_k^-=f(\hat{x}_{k-1},u_{k-1}) x^k−=f(x^k−1,uk−1)计算以下导数： A k = ∂ f ∂ x ∣ x ^ k − , u k G k = ∂ f ∂ w ∣ x ^ k − , u k C k = ∂ g ∂ x ∣ x ^ k − S k = ∂ 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−并顺带计算 P k − = A k P A k T + G k Q G k T P_k^-=A_kPA_k^T+G_kQG_k^T Pk−=AkPAkT+GkQGkT计算EKF的最优增益 K k = P k − C k T ( C k P k − C k T + S k R S k T ) − 1 K_k=P_k^-C_k^T(C_kP_k^-C_k^T+S_kRS_k^T)^{-1} Kk=Pk−CkT(CkPk−CkT+SkRSkT)−1更新 P k = ( I − K k C k ) P k − P_k=(I-K_kC_k)P_k^- Pk=(I−KkCk)Pk−并计算状态变量的后验估计 x ^ k = x ^ k − + K k ( y v − g ( x k − ) ) \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 + 1 k=k+1 k=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*1 yv = g(x0,v(i)); % 含噪声的测量 1*1 xfe = 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*1 P = (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}_0 x^0与真实值 x 0 x_0 x0相差较大造成EKF的误差较测量值较大，但是经过一段时间推移后，EKF的测量误差逐渐减小并较测量值有了显著提升。EKF算法对于非线性系统是近似收敛的，从测量方差的走势也可以看出。需要指出的是这里的测量方差是单位质量的动能，真实的动能需要乘上汽车的质量。 \qquad 希望本文对您有帮助，谢谢阅读。若有异议，欢迎评论区指正！ |
\qquad
其中
x
^
\hat{x}
x^为估计状态，
w
w
w为过程噪声（一般由控制变量
u
u
u引入，但也可能由物理系统本身的不确定性引入），而
v
v
v为测量噪声。根据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)) 从上述表达式也可以看出加入型噪声是非加入型的特例。
【本文地址】
今日新闻 |
推荐新闻 |