运动模型非线性扩展卡尔曼跟踪融合滤波算法（Matlab仿真）

????????卡尔曼滤波的原理和理论在CSDN已有很多文章，这里不再赘述，仅分享个人的理解和Matlab仿真代码。

1 单目标跟踪

????????匀速转弯（CTRV）运动模型下，摄像头输出目标状态camera_state = [x, y, theta, v]，雷达输出目标状态radar_state = [x, y, theta, v]。如果状态为[x, y, vx, vy]，也可以转成[x, y, theta, v]。其中theta=atan(vy/vx)，v=sqrt(vx*vx + vy*vy)，测量噪声设置要相应改变。

????????目标运动状态可以表示为：

????????由于存在非线性，可以用一阶泰勒展开的雅可比矩阵做线性化，考虑到w的除0情况，区分w=0或w≠0的结果。

????????w≠0时，

????????w=0时，

????????估计值和测量值为线性关系，状态观测矩阵可以用下面的矩阵表示。

????????

????????和线性卡尔曼滤波的对比如下：

????????这里由于测量和估计是线性关系，因此后验估计和卡尔曼滤波一样，直接用H矩阵。将上述公式用matlab编程即可得到滤波结果。

% 匀速转弯运动模型的扩展卡尔曼滤波算法仿真
% 目标的测量值为x,y,theta,v
clc;clear;close all;

% 匀速转弯运动的初始值
x0 = 0;                                     % 目标的初始横向位置
y0 = 0;                                     % 目标的初始纵向位置
theta = 0;                                  % 目标的偏航角（目标在当前坐标系下和x轴的夹角）
v = 3;                                      % 目标的速度
omga = 0.1;                                 % 目标的偏航角速度
N = 150;                                    % 数据量
dt = 0.2;                                   % 单帧时间
t = dt*(1:1:N);                             % 时间轴

% 更新超参数
sigma_q_x = 0.2;
sigma_q_y = 0.2;
sigma_q_theta = 0.01;
sigma_q_v = 0.1;
sigma_q_omga = 0.01;
Q_mat_ctrv = diag([sigma_q_x^2, sigma_q_y^2, sigma_q_theta^2, sigma_q_v^2, sigma_q_omga^2]);
sigma_R_ctrv = 0.4;
R_ctrv = diag([sigma_R_ctrv^2, sigma_R_ctrv^2, sigma_R_ctrv^2, sigma_R_ctrv^2]);

% 初始化参数
Xest_ekf = zeros(5, N);
P_ekf = zeros(5, 5, N);
P_ekf(:,:,1) = eye(5);
Z_ctrv = zeros(4, N);                               % 4维测量向量
H_ctrv = [1 0 0 0 0;
          0 1 0 0 0;
          0 0 1 0 0;
          0 0 0 1 0];
X_ctrv = zeros(5, N);
X_ctrv(:,1) = [1; 1; 0; 1; 0.1];                    % 初始状态，包括位置、偏航角、速度和偏航角速度
V_ctrv = mvnrnd(zeros(1,4), R_ctrv)';               % 观测误差矩阵
Z_ctrv(:,1) = H_ctrv * X_ctrv(:, 1) + V_ctrv;
Xest_ekf(1:4,1) = Z_ctrv(:,1);

% 扩展卡尔曼滤波的核心算法
for i = 2:N
    % 状态更新
    X_ctrv(:,i) = ekf_predict(X_ctrv(:,i-1), dt);
    W_ctrv = mvnrnd(zeros(1,5), Q_mat_ctrv)';       % 过程噪声向量
    X_ctrv(:,i) = X_ctrv(:,i) + W_ctrv;             % 加过程噪声

    % 预测步骤
    Xest_ekf(:,i) = ekf_predict(Xest_ekf(:,i-1), dt);
    F = ekf_jacobian(Xest_ekf(:,i-1), dt);
    P_ekf(:,:,i) = F * P_ekf(:,:,i-1) * F' + Q_mat_ctrv;

    % 测量模型更新
    V_ctrv = mvnrnd(zeros(1,4), R_ctrv)';           % 观测误差矩阵
    Z_ctrv(:,i) = H_ctrv * X_ctrv(:, i) + V_ctrv;

    % 更新步骤
    H_ekf = H_ctrv;                                 % 测量模型的雅可比矩阵
    K_ekf = P_ekf(:,:,i) * H_ekf' / (H_ekf * P_ekf(:,:,i) * H_ekf' + R_ctrv);
    Xest_ekf(:,i) = Xest_ekf(:,i) + K_ekf * (Z_ctrv(:,i) - H_ekf * Xest_ekf(:,i));
    P_ekf(:,:,i) = (eye(5) - K_ekf * H_ekf) * P_ekf(:,:,i); 
end

% 绘图部分保持不变
figure;
size = 10;
width = 2;
% 位置曲线图
subplot(2,2,1);
plot(Z_ctrv(1,:),'.g','MarkerSize',size); hold on;              % 画出测量值
plot(Xest_ekf(1,:),'b','LineWidth',width);hold on;              % 画出最优估计值
plot(X_ctrv(1,:),'r','LineWidth',width);                        % 画出实际状态值
title('X位置状态曲线');
legend('位置测量值', '位置最优估计值', '实际位置');

% 位置曲线图
subplot(2,2,2);
plot(Z_ctrv(2,:),'.g','MarkerSize',size);hold on;               % 画出测量值
plot(Xest_ekf(2,:),'b','LineWidth',width);hold on;              % 画出最优估计值
plot(X_ctrv(2,:),'r','LineWidth',width);                        % 画出实际状态值
title('Y位置状态曲线');
legend('位置测量值', '位置最优估计值', '实际位置');

% 偏航角曲线图
subplot(2,2,3);
plot(Z_ctrv(3,:),'.g','MarkerSize',size); hold on;              % 画出测量值
plot(Xest_ekf(3,:),'b','LineWidth',width); hold on;             % 画出最优估计值
plot(X_ctrv(3,:),'r','LineWidth',width);                        % 画出实际状态值
title('偏航角状态曲线');
legend('偏航角测量值', '偏航角最优估计值', '实际偏航角');

% 速度曲线图
subplot(2,2,4);
plot(Z_ctrv(4,:),'.g','MarkerSize',size); hold on;              % 画出测量值
plot(Xest_ekf(4,:),'b','LineWidth',width); hold on;             % 画出最优估计值
plot(X_ctrv(4,:),'r','LineWidth',width);                        % 画出实际状态值
title('速度状态曲线');
legend('速度测量值', '速度最优估计值', '实际速度');

% 位置平面图
% figure;
% plot(Z_ctrv(1,:),Z_ctrv(2,:));hold on;          % 画出测量值
% plot(Xest_ekf(1,:),Xest_ekf(2,:));              % 画出最优估计值
% plot(X_ctrv(1,:),X_ctrv(2,:));hold on;          % 画出实际值
% title('目标运动曲线');
% legend('位置测量值', '位置最优估计值', '实际位置');

% 扩展卡尔曼滤波的预测模型
function x_pred = ekf_predict(x, dt)
    % CTRV模型的预测模型
    theta = x(3);
    v = x(4);
    omega = x(5);

    if omega == 0 % 避免除以0
        dx = v * cos(theta) * dt;
        dy = v * sin(theta) * dt;
    else
        dx = v/omega * (sin(theta + omega * dt) - sin(theta));
        dy = v/omega * (-cos(theta + omega * dt) + cos(theta));
    end

    dtheta = omega * dt;
    
    x_pred = x + [dx; dy; dtheta; 0; 0];              % 速度和转向率的变化假设为0
end

% 根据推导的公式计算状态转移雅可比矩阵
function F = ekf_jacobian(x, dt)
    theta = x(3);
    v = x(4);
    omega = x(5);

    F = eye(5);

    if omega == 0 % 避免除以0
        F(1, 3) = -v * sin(theta) * dt;
        F(1, 4) = cos(theta) * dt;
        F(2, 3) = v * cos(theta) * dt;
        F(2, 4) = sin(theta) * dt;
    else
        F(1, 3) = v/omega * (cos(theta + omega * dt) - cos(theta));
        F(1, 4) = 1/omega * (sin(theta + omega * dt) - sin(theta));
        F(1, 5) = v/omega^2 * (sin(theta) - sin(theta + omega * dt)) + v * dt * cos(theta + omega * dt)/omega;
        F(2, 3) = v/omega * (sin(theta + omega * dt) - sin(theta));
        F(2, 4) = 1/omega * (-cos(theta + omega * dt) + cos(theta));
        F(2, 5) = v/omega^2 * (-cos(theta) + cos(theta + omega * dt)) + v * dt * sin(theta + omega * dt)/omega;
    end

    F(3, 5) = dt;
end

????????下图仿真运行的结果，这个文件仿真了单个匀速转弯扩展卡尔曼滤波算法结果。

2 多传感器融合定位跟踪?

? ? ? ? 如果是两个或多个目标，类似线性卡尔曼跟踪融合滤波算法（Matlab仿真）-CSDN博客中的融合仿真算法，分别交错仿真摄像头和雷达目标，代码如下。

% 匀速转弯运动模型的扩展卡尔曼滤波算法仿真
% 摄像头和雷达的测量值为x,y,theta,v
clc;clear;close all;

% 匀速转弯运动的初始值
x0 = 0;                                     % 目标的初始横向位置
y0 = 0;                                     % 目标的初始纵向位置
theta = 0;                                  % 目标的偏航角（目标在当前坐标系下和x轴的夹角）
v = 3;                                      % 目标的速度
omga = 0.1;                                 % 目标的偏航角速度
N = 150;                                    % 数据量
dt = 0.2;                                   % 单帧时间
t = dt*(1:1:N);                             % 时间轴

% 更新超参数
sigma_q_x = 0.2;
sigma_q_y = 0.2;
sigma_q_theta = 0.01;
sigma_q_v = 0.1;
sigma_q_omga = 0.01;
Q_matrix_ctrv = diag([sigma_q_x^2, sigma_q_y^2, sigma_q_theta^2, sigma_q_v^2, sigma_q_omga^2]);
% 设置测量噪声协方差矩阵R，噪声来自测量的误差
sigma_r_x = 1.0;
sigma_r_y = 0.2; 
sigma_r_theta = 0.01; 
sigma_r_v = 0.5;
R_matrix_ctrv_camera = diag([sigma_r_x^2, sigma_r_y^2, sigma_r_theta^2, sigma_r_v^2]);
% 上面是摄像头噪声协方差矩阵，下面是雷达噪声协方差矩阵
sigma_r_x = 0.4;
sigma_r_y = 0.4; 
sigma_r_theta = 0.01; 
sigma_r_v = 0.1;
R_matrix_ctrv_radar = diag([sigma_r_x^2, sigma_r_y^2, sigma_r_theta^2, sigma_r_v^2]);

% 初始化参数
Xest_ekf = zeros(5, N);
P_ekf = zeros(5, 5, N);
P_ekf(:,:,1) = eye(5);
Z_ctrv = zeros(4, N);                               % 4维测量向量
H_ctrv = [1 0 0 0 0;
          0 1 0 0 0;
          0 0 1 0 0;
          0 0 0 1 0];
X_ctrv = zeros(5, N);
X_ctrv(:,1) = [1; 1; 0; 1; 0.1];                    % 初始状态，包括位置、偏航角、速度和偏航角速度
R_matrix_ctrv = R_matrix_ctrv_camera;               % 第1帧为摄像头
V_ctrv = mvnrnd(zeros(1,4), R_matrix_ctrv)';        % 观测误差矩阵
Z_ctrv(:,1) = H_ctrv * X_ctrv(:, 1) + V_ctrv;
Xest_ekf(1:4,1) = Z_ctrv(:,1);

% 扩展卡尔曼滤波的核心算法
for i = 2:N
    % 选择摄像头或雷达,摄像头是奇数帧，雷达是偶数帧
    if mod(i,2) == 1
        R_matrix_ctrv = R_matrix_ctrv_camera;
    else
        R_matrix_ctrv = R_matrix_ctrv_radar;
    end
    % 状态更新
    X_ctrv(:,i) = ekf_predict(X_ctrv(:,i-1), dt);
    W_ctrv = mvnrnd(zeros(1,5), Q_matrix_ctrv)';        % 过程噪声向量
    X_ctrv(:,i) = X_ctrv(:,i) + W_ctrv;                 % 加过程噪声

    % 预测步骤
    Xest_ekf(:,i) = ekf_predict(Xest_ekf(:,i-1), dt);
    F = ekf_jacobian(Xest_ekf(:,i-1), dt);
    P_ekf(:,:,i) = F * P_ekf(:,:,i-1) * F' + Q_matrix_ctrv;

    % 测量模型更新
    V_ctrv = mvnrnd(zeros(1,4), R_matrix_ctrv)';            % 观测误差矩阵
    Z_ctrv(:,i) = H_ctrv * X_ctrv(:, i) + V_ctrv;

    % 更新步骤
    H_ekf = H_ctrv;                                         % 测量模型的雅可比矩阵
    K_ekf = P_ekf(:,:,i) * H_ekf' / (H_ekf * P_ekf(:,:,i) * H_ekf' + R_matrix_ctrv);
    Xest_ekf(:,i) = Xest_ekf(:,i) + K_ekf * (Z_ctrv(:,i) - H_ekf * Xest_ekf(:,i));
    P_ekf(:,:,i) = (eye(5) - K_ekf * H_ekf) * P_ekf(:,:,i); 
end

% 绘图部分保持不变
figure;
size = 10;
width = 2;
% 位置曲线图
subplot(2,2,1);
plot(Z_ctrv(1,:),'.g','MarkerSize',size); hold on;              % 画出测量值
plot(Xest_ekf(1,:),'b','LineWidth',width);hold on;              % 画出最优估计值
plot(X_ctrv(1,:),'r','LineWidth',width);                        % 画出实际状态值
title('X位置状态曲线');
legend('位置测量值', '位置最优估计值', '实际位置');

% 位置曲线图
subplot(2,2,2);
plot(Z_ctrv(2,:),'.g','MarkerSize',size);hold on;               % 画出测量值
plot(Xest_ekf(2,:),'b','LineWidth',width);hold on;              % 画出最优估计值
plot(X_ctrv(2,:),'r','LineWidth',width);                        % 画出实际状态值
title('Y位置状态曲线');
legend('位置测量值', '位置最优估计值', '实际位置');

% 偏航角曲线图
subplot(2,2,3);
plot(Z_ctrv(3,:),'.g','MarkerSize',size); hold on;              % 画出测量值
plot(Xest_ekf(3,:),'b','LineWidth',width); hold on;             % 画出最优估计值
plot(X_ctrv(3,:),'r','LineWidth',width);                        % 画出实际状态值
title('偏航角状态曲线');
legend('偏航角测量值', '偏航角最优估计值', '实际偏航角');

% 速度曲线图
subplot(2,2,4);
plot(Z_ctrv(4,:),'.g','MarkerSize',size); hold on;              % 画出测量值
plot(Xest_ekf(4,:),'b','LineWidth',width); hold on;             % 画出最优估计值
plot(X_ctrv(4,:),'r','LineWidth',width);                        % 画出实际状态值
title('速度状态曲线');
legend('速度测量值', '速度最优估计值', '实际速度');

% 分别提取摄像头和雷达目标
% 确定摄像头和雷达帧数
if mod(N,2) == 0
    N_camera = N/2;
    N_radar = N/2;
else
    N_camera = floor(N/2) + 1;
    N_radar = floor(N/2);
end
Z_ctrv_camera = zeros(4,N_camera);
Z_ctrv_radar = zeros(4,N_radar);
camera_frame = zeros(1,N_camera);
radar_frame = zeros(1,N_radar);
camera_count = 0;
radar_count = 0;
% 提取摄像头和雷达帧的数据，摄像头在奇数帧，雷达在偶数帧
for i = 1:N
    if mod(i,2) == 1                                        
        camera_count = camera_count + 1;
        camera_frame(camera_count) = i;
        Z_ctrv_camera(:,camera_count) = Z_ctrv(:,i);            % 摄像头数据
    else
        radar_count = radar_count + 1;
        radar_frame(radar_count) = i;
        Z_ctrv_radar(:,radar_count) = Z_ctrv(:,i);              % 雷达数据
    end
end

% 绘图部分保持不变
figure;size = 10;width = 2;
% X位置曲线图
subplot(2,2,1);
plot(camera_frame,Z_ctrv_camera(1,:),'.g','MarkerSize',size); hold on;                % 画出测量值
plot(radar_frame,Z_ctrv_radar(1,:),'.k','MarkerSize',size); hold on;                  % 画出测量值
plot(Xest_ekf(1,:),'b','LineWidth',width);hold on;                                    % 画出最优估计值
plot(X_ctrv(1,:),'r','LineWidth',width);                                              % 画出实际状态值
title('X位置状态曲线');
legend('摄像头位置测量值', '雷达位置测量值','位置最优估计值', '实际位置');

% Y位置曲线图
subplot(2,2,2);
plot(camera_frame,Z_ctrv_camera(2,:),'.g','MarkerSize',size); hold on;                % 画出测量值
plot(radar_frame,Z_ctrv_radar(2,:),'.k','MarkerSize',size); hold on;                  % 画出测量值
plot(Xest_ekf(2,:),'b','LineWidth',width);hold on;                                    % 画出最优估计值
plot(X_ctrv(2,:),'r','LineWidth',width);                                              % 画出实际状态值
title('Y位置状态曲线');
legend('摄像头位置测量值', '雷达位置测量值','位置最优估计值', '实际位置');

% 偏航角曲线图
subplot(2,2,3);
plot(camera_frame,Z_ctrv_camera(3,:),'.g','MarkerSize',size); hold on;                % 画出测量值
plot(radar_frame,Z_ctrv_radar(3,:),'.k','MarkerSize',size); hold on;                  % 画出测量值
plot(Xest_ekf(3,:),'b','LineWidth',width); hold on;                                   % 画出最优估计值
plot(X_ctrv(3,:),'r','LineWidth',width);                                              % 画出实际状态值
title('偏航角状态曲线');
legend('偏航角测量值', '偏航角测量值', '偏航角最优估计值', '实际偏航角');

% 速度曲线图
subplot(2,2,4);
plot(camera_frame,Z_ctrv_camera(4,:),'.g','MarkerSize',size); hold on;                % 画出测量值
plot(radar_frame,Z_ctrv_radar(4,:),'.k','MarkerSize',size); hold on;                  % 画出测量值
plot(Xest_ekf(4,:),'b','LineWidth',width); hold on;                                   % 画出最优估计值
plot(X_ctrv(4,:),'r','LineWidth',width);                                              % 画出实际状态值
title('速度状态曲线');
legend('摄像头速度测量值', '雷达速度测量值', '速度最优估计值', '实际速度');

% 位置平面图
% figure;
% plot(Z_ctrv(1,:),Z_ctrv(2,:));hold on;          % 画出测量值
% plot(Xest_ekf(1,:),Xest_ekf(2,:));              % 画出最优估计值
% plot(X_ctrv(1,:),X_ctrv(2,:));hold on;          % 画出实际值
% title('目标运动曲线');
% legend('位置测量值', '位置最优估计值', '实际位置');

% 扩展卡尔曼滤波的预测模型
function x_pred = ekf_predict(x, dt)
    % CTRV模型的预测模型
    theta = x(3);
    v = x(4);
    omega = x(5);

    if omega == 0 % 避免除以0
        dx = v * cos(theta) * dt;
        dy = v * sin(theta) * dt;
    else
        dx = v/omega * (sin(theta + omega * dt) - sin(theta));
        dy = v/omega * (-cos(theta + omega * dt) + cos(theta));
    end

    dtheta = omega * dt;
    
    x_pred = x + [dx; dy; dtheta; 0; 0]; % 速度和转向率的变化假设为0
end

function F = ekf_jacobian(x, dt)
    theta = x(3);
    v = x(4);
    omega = x(5);

    F = eye(5);

    if omega == 0 % 避免除以0
        F(1, 3) = -v * sin(theta) * dt;
        F(1, 4) = cos(theta) * dt;
        F(2, 3) = v * cos(theta) * dt;
        F(2, 4) = sin(theta) * dt;
    else
        F(1, 3) = v/omega * (cos(theta + omega * dt) - cos(theta));
        F(1, 4) = 1/omega * (sin(theta + omega * dt) - sin(theta));
        F(1, 5) = v/omega^2 * (sin(theta) - sin(theta + omega * dt)) + v * dt * cos(theta + omega * dt)/omega;
        F(2, 3) = v/omega * (sin(theta + omega * dt) - sin(theta));
        F(2, 4) = 1/omega * (-cos(theta + omega * dt) + cos(theta));
        F(2, 5) = v/omega^2 * (-cos(theta) + cos(theta + omega * dt)) + v * dt * sin(theta + omega * dt)/omega;
    end

    F(3, 5) = dt;
end

? ? ? ?仿真代码给出摄像头和雷达目标定位跟踪融合的结果，第一张图的测量值没有区分摄像头和雷达，第二张图做了区分。