拓展卡尔曼滤波分析与实现

      拓展卡尔曼滤波由线性卡尔曼滤波发展而来。虽然线性卡尔曼滤波存在只能处理线性系统的局限性，但是拓展卡尔曼滤波解决了这一问题，而且在非线性系统中经常取得较好的吻合。当然拓展卡尔曼滤波比线性卡尔曼滤波实现起来也更加困难，目前网上的资料也不是很多，特别是关于MATLAB的实现资料很少。

 

注意：1、本文中，时间k作为下标而不是放在括号内。

            2、本文作为拓展卡尔曼滤波分析，默认读者已经至少简单了解线性卡尔曼滤波相关知识。

           3、拓展卡尔曼滤波公式繁多，具有极强劝退效果。本文也不能免俗，唯希望读者可以跟着步骤走。如果难理解，希望多读几遍。作者虽自觉水平有限，但亦觉得比网上大多文章透彻清晰。

 

• 关于线性卡尔曼滤波（简单带过）。

下面是线性卡尔曼滤波的五个方程。如果有读者连线性卡尔曼也不熟悉，建议bilibili看一些讲解视频，我个人觉得vic讲的很不错，他在视频后面编写了线性卡尔曼的MATLAB实现程序。

• 拓展卡尔曼滤波

这是本文的重头戏，也将会花最大篇幅来详细讲解拓展卡尔曼滤波。

拓展卡尔曼滤波困难之处就在于线性化的过程。本例以二维小车运动为例来进行讲解。（本例中状态变化仍为线性，但是观测是非线性）

 

1、 首先生成一组数据作为小车真实运动轨迹。

物理模型基本假定：匀速运动，但存在随机扰动(将很多物理因素的共同作用归结为数学模型中的随机扰动)作为加速度。

 

建立状态向量

                                                                               （其中  即是  即为速度，y方向同理）

根据高中物理知识可得状态转移方程（即为，即加速度扰动，y方向同理）

写成矩阵形式即为                                                 

其中

由此即可得到小车的运动轨迹。

 

2、然后开始进行EKF滤波估计

先理解一下EKF的方程（其中为符合雅克比矩阵习惯，式6.25 更常写作）

和线性卡尔曼滤波一样，拓展卡尔曼滤波同样分为两个部分（预测和测量更新）。但是拓展卡尔曼滤波处理的是非线性系统，所以不能以矩阵相乘必须以函数形式来更一般的表示上一时刻到现在时刻的变化。本例中状态向量是s而非x，观测向量是（r，b）而非y。并且本例中的状态变化是线性的，但是观测是非线性的。

因为状态变化是线性的，所以预测部分非常简单

在循环中

   s_ = A_*s(:,i-1);

   P_ = A_*P*A_'+G_*Q_*G_';

如上两行就可以吻合拓展卡尔曼滤波方程的预测部分。

 

重点是观测更新部分，函数h是非线性的，所以无法直接用矩阵运算来实现观测更新。但是我们有办法把非线性转化为线性。

在大学课程《高等数学》中我们学过，非线性函数y=h(x)可通过泰勒公式在点(x0,y0)处展开为泰勒级数：

忽略掉二次项及以上，即可留下线性方程，来代替变化函数h(x)。

将非线性函数h(x)拓展到多维，即求各个变量的偏导数（本例即为二维小车的运动），即：

对x求偏导数所对应的这一项被称为雅可比（Jacobian）式。

 

我们可以直接雅克比矩阵结论：

在本例中h(x)：

由求偏导公式可以得出本例雅克比矩阵为：

即为x,y，为。

有了雅可比矩阵，即可由 式6.27 求得卡尔曼增益。

 

另外单独说一下式6.28。

式6.28 红色部分即为最小均方误差MMSE。是观测值（本例在第一部分生成），是预测部分 式6.23 得到的估计值进行h变换结果，换句话说就是把式6.23得到的估计值带入h(x)，生成新的观测向量。再用观测值相减从而得到MMSE。

式6.29 非常普通，简单带入即可。

 

• 本例初始值即环境参数设置

为防止某些小白看到接近终点放弃，多嘴一句MVN是多维正态分布，大写I是单位矩阵。2，3部分是扰动服从MVN分布。这一点在代码中体现更加直观。

 

• MATLAB代码

clear;
close all;

%% 设置初始值和环境
MarkerSize = 15; FontSize = 25; LineWidth = 2;

S(:,1) = [10;-0.2;-5;0.2];               %大写S矩阵保存真实值
no_noise(:,1) = [10;-0.2;-5;0.2];        %no_noise保存理想值
s(:,1) = [5;0;5;0];                      %小写s矩阵保存EKF估计值
T = 1;

A = [1 T 0 0; 0 1 0 0; 0 0 1 T; 0 0 0 1];%状态转移矩阵
G = [1/2*T^2 0; T 0; 0 1/2*T^2; 0 T];    %加速度叠加矩阵

%% 生成真实值和观测值rk bk
for i=1:500
    u(:,i) = mvnrnd([0,0],[0.0001,0;0,0.0001])';                        %随机噪声
    t(:,i) = mvnrnd([0,0],[0.01,0;0,0.01])';                            %测量噪声
    YY(:,i) = [sqrt(S(1,i)^2+S(3,i)^2);atan2(S(3,i),S(1,i))]+t(:,i);    %生成观测值
    S(:,i+1) = A*S(:,i)+G*u(:,i);                                       %生成真实值
    no_noise(:,i+1) = A*no_noise(:,i);                                  %生成理想值
    
end

figure(1)
% 取消注释可以绘制理想轨迹
% plot(no_noise(1,2: end), no_noise(3,2: end),'r', 'LineWidth',LineWidth, 'MarkerSize',MarkerSize)
% axis equal
% hold on                     %绘制理想轨迹

plot(S(1,2: end), S(3,2: end),'-', 'LineWidth',LineWidth, 'MarkerSize',MarkerSize)
axis equal
hold on                     %绘制真实轨迹

A_ = A; G_ = G;
P = 100*eye(4);

Q_ = 1e-4*eye(2);
R = diag([0.01, 0.01]);     %观测噪声协方差矩阵


%% 根据卡尔曼方程来进行滤波
for i = 2:500
    %预测
   s_ = A_*s(:,i-1);
   P_ = A_*P*A_'+G_*Q_*G_'; 
    %更新
   px = s_(1); py = s_(3); 
        % 雅克比矩阵（第一次计算雅克比矩阵尝试）
        %    H = [px/(px^2+py^2)^(1/2) py/(px^2+py^2)^(1/2) 0 0;
        %         -py/(px^2+py^2)      px/(px^2+py^2)       0 0;
        %         py*(vx*py-vy*px)/(px^2+py^2)^(3/2) px*(vy*px-vx*py)/(px^2+py^2)^(3/2) px/(px^2+py^2)^(1/2) py/(px^2+py^2)^(1/2)];
   H = [px/(px^2+py^2)^(1/2) 0 py/(px^2+py^2)^(1/2) 0;
        -py/(px^2+py^2)      0 px/(px^2+py^2)       0];  %雅克比矩阵
   Mk = P_*H'/(H*P_*H'+R);          %卡尔曼增益
   y = [(s_(1)^2+s_(3)^2)^(1/2);atan2(s_(3),s_(1))];
   s(:,i) = s_+Mk*(YY(:,i)-y);
   P = P_ - Mk*H*P_;
   
end

plot(s(1,2: end), s(3,2:end),':', 'LineWidth',LineWidth, 'MarkerSize',MarkerSize)
% legend('理想轨迹','真实轨迹', 'EKF估计')
legend('真实轨迹', 'EKF估计')
grid on; ax = gca; ax.FontSize = FontSize;


%% 绘制rk和bk
r_k = YY(1,2:end); b_k = YY(2,2:end);
figure(2)
subplot(211)
plot(r_k);
legend('r_k');
subplot(212)
plot(b_k,'r');
legend('b_k');

%% 绘制rk，bk计算轨迹和真实轨迹对比
figure(3)
xr = r_k.*cos(b_k);
yr = r_k.*sin(b_k);
plot(yr,xr)
hold on
plot(S(1,2: end), S(3,2: end),'-', 'LineWidth',LineWidth, 'MarkerSize',MarkerSize)
legend('根据r_k,b_k计算轨迹','真实轨迹');