运动模型非线性扩展卡尔曼跟踪融合滤波算法（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仿真代码给出摄像头和雷达目标定位跟踪融合的结果，第一张图的测量值没有区分摄像头和雷达，第二张图做了区分。
