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

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:
$$\{\begin{array}{l}x(k)=f(x(k-1),u(k-1))\\ y(k)=g(x(k),u(k))\end{array}$$
引入EKF后，其结构框图如下：

\qquad 其中$\hat{x}$为估计状态，$w$为过程噪声（一般由控制变量$u$引入，但也可能由物理系统本身的不确定性引入），而$v$为测量噪声。根据Matlab的帮助文档介绍，噪声也分为两种——加入型噪声（Additive Noise）和非加入型噪声（Nonadditive Noise），其分类取决于噪声是否在非线性函数内部，二者的状态方程形式如下：
Additive Noise:
$$\{\begin{array}{l}x(k)=f(x(k-1),u(k))+w(k)\\ y(k)=g(x(k),u(k))+v(k)\end{array}$$
Nonadditive Noise
$$\{\begin{array}{l}x(k)=f(x(k-1),u(k),w(k))\\ y(k)=g(x(k),u(k),v(k))\end{array}$$
从上述表达式也可以看出加入型噪声是非加入型的特例。

1.场景预设

\qquad 为了应用EKF，需要构造一个非线性系统，与前一篇讲述KF的博文保持连续性。这次使用的仍然所示一维的汽车运动模型，状态变量仍然选择汽车的位移和速度$(x=[p,v{]}^T)$，但这次控制变量为$u(k)$为汽车的功率/汽车的质量，重新构造状态方程如下：
$$\{\begin{array}{l}\dot{x}=f(x,u)=A_0^{\ast }x+B_0^{\ast T}u/(B_0^{\ast }x)\\ y=g(x)=\frac{1}{2}(C_0^{\ast }x{)}^T(C_0^{\ast }x)\end{array}$$
其中$$A_0^{\ast }=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right],B_0^{\ast }=C_0^{\ast }=\left[\begin{array}{cc}0& 1\end{array}\right]$$
为了与控制变量$u$保持一致性，此处的测量$y$为单位质量产生的动能。为了不失一般性，添加非加入型噪声如下（同样以$y_v$表示测量值，$y$表示实际值，$\hat{y}$表示估计值）：
$$\{\begin{array}{l}\dot{x}=f(x,u)=A_0^{\ast }x+B_0^{\ast T}(u+w)/(B_0^{\ast }x)\\ y_v=g(x)=\frac{1}{2}(C_0^{\ast }x{)}^T(C_0^{\ast }x)+v\end{array}$$
设定采样时间$dt$，状态方程化为离散形式：
$$\{\begin{array}{l}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))=\frac{1}{2}(C_{0}x(n){)}^T(C_{0}x(n))+v\end{array}$$
其中$$A_0=\left[\begin{array}{cc}1& dt\\ 0& 1\end{array}\right],B_0=\left[\begin{array}{cc}0& 1\end{array}\right]$$
与上一篇博文不同，设$$E((B_{0}w-\overline{B_{0}w}{)}^T(B_{0}w-\overline{B_{0}w}))=Q,E((v-\overline{v}{)}^T(v-\overline{v}))=R$$

2.扩展卡尔曼滤波器

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

1. 设定初始状态变量的估计值${\hat{x}}_0$，并计算以下导数及P的初始值：$$\begin{gathered} A_0=\frac{\partial f}{\partial x}{|}_{{\hat{x}}_0,u_0} \\ {G}_0=\frac{\partial f}{\partial w}{|}_{{\hat{x}}_0,u_0} \\ {C}_0=\frac{\partial g}{\partial x}{|}_{{\hat{x}}_0} \\ {S}_0=\frac{\partial g}{\partial v}{|}_{{\hat{x}}_0} \\ {P}_0=G_{0}Q{G}_0^T \end{gathered}$$令$k=1$
2. 获取当前测量量$y_v$，计算状态变量的先验估计${\hat{x}}_k^{-}=f({\hat{x}}_{k-1},u_{k-1})$
3. 计算以下导数：$$\begin{gathered} A_k=\frac{\partial f}{\partial x}{|}_{{\hat{x}}_k^{-},u_k} \\ {G}_k=\frac{\partial f}{\partial w}{|}_{{\hat{x}}_k^{-},u_k} \\ {C}_k=\frac{\partial g}{\partial x}{|}_{{\hat{x}}_k^{-}} \\ {S}_k=\frac{\partial g}{\partial v}{|}_{{\hat{x}}_k^{-}} \end{gathered}$$并顺带计算$P_k^{-}=A_{k}P{A}_k^T+G_{k}Q{G}_k^T$
4. 计算EKF的最优增益$K_k=P_k^{-}{C}_k^T(C_k{P}_k^{-}{C}_k^T+S_{k}R{S}_k^T{)}^{-1}$
5. 更新$P_k=(I-K_k{C}_k)P_k^{-}$并计算状态变量的后验估计${\hat{x}}_k={\hat{x}}_k^{-}+K_k(y_v-g(x_k^{-}))$
6. 计算测量量的估计值${\hat{y}}_k=g({\hat{x}}_k)$，令$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 通过效果图可以看出，虽然初始状态时估计值${\hat{x}}_0$与真实值$x_0$相差较大造成EKF的误差较测量值较大，但是经过一段时间推移后，EKF的测量误差逐渐减小并较测量值有了显著提升。EKF算法对于非线性系统是近似收敛的，从测量方差的走势也可以看出。需要指出的是这里的测量方差是单位质量的动能，真实的动能需要乘上汽车的质量。
\qquad 希望本文对您有帮助，谢谢阅读。若有异议，欢迎评论区指正！