扩展卡尔曼滤波——非线性EKF-Matlab

扩展卡尔曼滤波EKF仿真实例
这里以无人驾驶的测量障碍物的实际案例为例子展开，如下图所示，毫米波雷达能够测量障碍物在极坐标下离雷达的距离 、方向角以及距离的变化率（径向速度） ，如下图所示，

预测更新
以二维匀速运动为例，这里的状态量为：

根据牛顿运动定理，经过 [公式] 时间后的预测状态量为：

对于二维的匀速运动模型，加速度为10，并会对预测后的状态造成影响.。
由此可得状态转移矩阵为

毫米波雷达测量障碍物在径向上的位置和速度相对准确，不确定度较低，因此可以对状态协方差矩阵P进行如下初始化：

由于Q对整个系统存在影响，但又不能太确定对系统的影响有多大。对于简单的模型来说，这里可以直接使用简单矩阵进行计算。

量测更新
由图1可知观测值z的数据维度为3×1，为了能够实现矩阵运算，y和Hx’的数据维度也都为3×1。

使用基本的数学运算可以完成预测值x’从笛卡尔坐标系到极坐标系的坐标转换，求得Hx’，再与观测值z进行计算。需要注意的是，在计算差值y的这一步中，我们通过坐标转换的方式避开了未知的旋转矩阵H的计算，直接得到了Hx’的值。

为了简化表达，我们用px，py以及vx和vy表示预测后的位置及速度，如下所示：

毫米波雷达观测z是包含位置、角度和径向速度的3x1的列向量，状态向量x’是包含位置和速度信息的4x1的列向量，根据公式y=z-Hx’可知测量矩阵（Measurement Matrix）H的维度是3行4列。即：

[此时，我们需要对H矩阵进行泰勒展开（线性化），得到其Jacobian矩阵，

经过线性化，最终得到测量矩阵H为：

标准卡尔曼滤波仿真实例请看上篇 ：
(卡尔曼滤波——线性定常) https://editor.csdn.net/md/?articleId=104242921

下面是Matlab代码实现。
注：本例所用传感器有激光雷达传感器，雷达传感器

%EKF无味卡尔曼滤波在自动驾驶车辆定位中的应用
%参考文献:
%        The High-Degree Cubature Kalman Filter (Bin Jia, Ming Xin, and Yang Cheng)
%        Unscented Kalman Filter Tutorial (Gabriel A. Terejanu )
%        基于正交变换的五阶容积卡尔曼滤波导航算法 (何康辉，董朝阳 )
%        ＥＫＦ、ＵＫＦ和ＣＫＦ的滤波性能对比研究 (常宇健，赵辰)
%        组合导航原理与应用 (西北工业大学出版社)
%        卡尔曼滤波与Matlab仿真 (北京航空航天大学出版社)
% 本例子使用线性状态模型，因此加速度变成了干扰项
% Lidar传感器测量模型为线性模型，Radar传感器测模型为非线性模型
% 在Radar传感器中使用滤波算然为 EKF,在Lidar传感器中使用滤波算然为 KF
clear;
clc;
tic;      %计时
dt = 0.1; %步长

%分配空间
Radar_measurement = [];
Radar_measurement_p = [];
Lidar_measurement = [];
EKF_estimation = [];
ekf = [];  % 结构体

%读取Radar_Lidar_Data2.csv文件保存到矩阵Data
Data = csvread('Radar_Lidar_Data1.csv',1,1);%起点为第2行第2列    612组数据
%将Data矩阵数据保存到data.csv文件中
csvwrite('Data.csv',Data);

%初始化状态，x是4维向量
if (Data(1,1) == 1)  
    x = [Data(1,2); Data(1,3); 0; 0];          %如果是lidar测量数据，文件 Dada2.csv，保存这个
else
    x = [   
            Data(1,2)*cos(Data(1,3)); 
            Data(1,2)*sin(Data(1,3)); 
            Data(1,4)*cos(Data(1,3)); 
            Data(1,4)*sin(Data(1,3))
         ];  %如果是Radar测量数据，文件 Dada1.csv，保存这个
end

%初始化状态协方差矩阵
P = diag([1,1,1000,1000]);
 
%状态转移矩阵，线性
F = [[1, 0, dt, 0];
     [0, 1, 0, dt];
     [0, 0, 1, 0];
     [0, 0, 0, 1]];

 %系统误差协方差矩阵
Q = [[(dt^2)/4,0,(dt^3)/2,0];
     [0,(dt^2)/4,0,(dt^3)/2];
     [(dt^3/2),0,(dt^2),0];
     [0,(dt^3)/2,0,(dt^2)]];
 
%Lidar传感器观测矩阵，线性
H_L = [[1, 0, 0, 0];
       [0, 1, 0, 0]];

%雷达传感器测量误差协方差矩阵
R_r = diag([0.09,0.005,0.09]);

%激光雷达传感器测量误差协方差矩阵
R_L = diag([0.0025, 0.0025]);

%4阶单位矩阵
I = eye(4);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%开始迭代%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for n = 1:length(Data)
    
    %Radar传感器
    if (Data(n,1) == 2)
        
        %Radar测量数据用于作图比较
        z = transpose(Data(n,2:4));
        z_p = [ z(1) * cos(z(2));z(1) * sin(z(2));z(3) * cos(z(2));z(3) * sin(z(2))];
        
        %1.预测更新状态、状态协方差阵
        x = F * x;
        P = F * P * transpose(F) + Q;
        
        %2.计算雅可比矩阵
        c1 = x(1)^2 + x(2)^2;
        c2 = sqrt(c1);
        c3 = c1 * c2;
        %测量方程非线性映射
        h = @(x)[c2;
                 atan(x(2)/x(1));
                 (x(1)*x(3)+x(2)*x(4))/c2];
        %避免除0情况
        if (c1==0 || c2==0 || c3==0)
            H_Jac = [[0, 0, 0, 0];
                     [0, 0, 0, 0];
                     [0, 0, 0, 0]];
        else  
            H_Jac = [[x(1)/c2, x(2)/c2, 0, 0];
                    [-x(2)/c1, x(1)/c1, 0, 0];
                    [(x(2)*(x(3)*x(2)-x(4)*x(1)))/c3, (x(1)*(x(1)*x(4)-x(2)*x(3)))/c3, x(1)/c2, x(2)/c2]];
        end
        
        %3.测量更新
        y = z - h(x);  
        %更新卡尔曼增益矩阵
        S = H_Jac * P * transpose(H_Jac) + R_r;
        K = P * transpose(H_Jac) / S;
        %测量更新状态
        x = x + (K * y);
        %更新状态协方差阵
        P = (I - (K * H_Jac)) * P;
        
        %5.保存状态值
        EKF_estimation = [EKF_estimation;transpose(x)];
        Radar_measurement = [Radar_measurement; transpose(z)];
        Radar_measurement_p = [Radar_measurement_p;transpose(z_p)];
    
    %Lidar传感器
    %线性
    else  %(Data(n,1) == 1)情况
        
        %1.预测更新状态、状态协方差阵
        x = F * x;
        P = F * P * transpose(F) + Q;

        %2.Radar测量数据
        z = transpose(Data(n,2:3));
        
        %3.测量更新
        %更新卡尔曼增益矩阵
        y = z - (H_L * x);
        S = H_L * P * transpose(H_L) + R_L;
        K = P * transpose(H_L) / S;
        %测量更新状态
        x = x + (K * y);
        %更新状态协方差阵
        P = (I - (K * H_L)) * P;
        
        %4.保存状态值
        EKF_estimation = [EKF_estimation;transpose(x)];
        Lidar_measurement = [Lidar_measurement; transpose(z)];
    end
    
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 数据可视化%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(1);
hold on;
plot(Data(:,5),Data(:,6),'k');                                                %真实状态
scatter(EKF_estimation(:,1),EKF_estimation(:,2),5,'filled','r');                          %估计状态
scatter(Lidar_measurement(:,1),Lidar_measurement(:,2),5,'filled','blue');     %Lidar传感器测量
scatter(Radar_measurement_p(:,1),Radar_measurement_p(:,2),5,'filled','g');    %Radar传感器测量
legend('truth','EKF Path result','Lidar Measurement','Radar Measurement');    %标注
title('位置估计');
axis square;
grid on;
hold off;

%位置x误差
figure(2);
hold on;
EKF_x_error = Data(1:2:1223,5) - EKF_estimation(1:2:1223,1);  %位置x误差
plot(1:612,EKF_x_error);
plot(1:612,Data(1:2:1223,5) - Radar_measurement_p(:,1));
legend('EKFerror','Radarerror');       
title('位置x误差');
grid on;
hold off;

%位置y误差
figure(3);
hold on;
EKF_y_error = Data(1:2:1223,6) - EKF_estimation(1:2:1223,2);  %位置y误差
plot(1:612,EKF_y_error);
plot(1:612,Data(1:2:1223,6) - Radar_measurement_p(:,2));
legend('EKFerror','Radarerror');       
title('位置y误差');
grid on
hold off;

%速度x误差
figure(4);
hold on;
EKF_vx_error = Data(1:2:1223,7) - EKF_estimation(1:2:1223,3);  %速度x误差
plot(1:612,EKF_vx_error);
plot(1:612,Data(1:2:1223,7) - Radar_measurement_p(:,3));
legend('EKFerror','Radarerror');       
title('速度x误差');
grid on
hold off;

%速度y误差
figure(5);
hold on;
EKF_vy_error = Data(1:2:1223,8) - EKF_estimation(1:2:1223,4);  %速度y误差
plot(1:612,EKF_vy_error);
plot(1:612,Data(1:2:1223,8) - Radar_measurement_p(:,4));
legend('EKFerror','Radarerror');       
title('速度y误差');
grid on
hold off;

%将数据保存到 EKF_error.csv 文件中,用于软法比较
EKF_error = [EKF_x_error,EKF_y_error,EKF_vx_error,EKF_vy_error];
csvwrite('EKF_error.csv',EKF_error);
%保存到结构体
ekf.estimation = EKF_estimation;
ekf.error      = EKF_error;
ekf.measurement_R = Radar_measurement_p;
ekf.measurement_L = Lidar_measurement;
%结束计时
toc;

输出结果：