线性卡尔曼跟踪融合滤波算法（Matlab仿真）
卡尔曼滤波的原理和理论在CSDN已有很多文章，这里不再赘述，仅分享个人的理解和Matlab仿真代码。
1 单目标跟踪
假设目标的状态为X = [x, y, vx, vy]，符合匀速直线运动目标，也即
其中F为状态转移矩阵，
在匀速直线(const velocity)运动模型时，整个系统为线性状态，可以直接调用卡尔曼滤波的几个公式
考虑到实际测量值的状态，Z=[x, y, vx, vy]，观测矩阵可以写作
如果测量值Z = [x,y]，则
将上述过程用Matlab编程仿真，则可以得到目标跟踪滤波的结果。
% 匀速直线运动模型的卡尔曼滤波算法仿真
% 测量值为x,y,vx,vy
clc;clear;close all;
% 匀速直线运动的初始值
x0 = 0; % 目标的初始横向位置
y0 = 0; % 目标的初始纵向位置
vx0 = 1; % 目标的横向速度
vy0 = 1; % 目标的纵向速度
N = 150; % 数据量
dt = 0.2; % 单帧时间
t = dt*(1:1:N); % 时间轴
% 设置过程噪声协方差矩阵Q，噪声来自真实世界中的不确定性，代表运动模型和实际运动的偏差
sigma_q_x = 0.1; % 纵向距离的过程噪声标准差
sigma_q_y = 0.1; % 横向距离的过程噪声标准差
sigma_q_vx = 0.1; % 纵向速度的过程噪声标准差
sigma_q_vy = 0.1; % 横向速度的过程噪声标准差
Q_matrix_cv = diag([sigma_q_x^2, sigma_q_y^2, sigma_q_vx^2, sigma_q_vy^2]);
% 设置测量噪声协方差矩阵R，噪声来自测量的误差
sigma_r_x = 0.5;
sigma_r_y = 0.5;
sigma_r_vx = 0.1;
sigma_r_vy = 0.1;
R_matrix_cv = diag([sigma_r_x^2, sigma_r_y^2, sigma_r_vx^2, sigma_r_vy^2]);
% 卡尔曼滤波器初始化参数
X_cv = zeros(4,N); % 初始化目标运动真值
W_cv = mvnrnd(zeros(1,4), Q_matrix_cv)'; % 4维过程噪声向量
F_cv = [1 0 dt 0;
0 1 0 dt;
0 0 1 0;
0 0 0 1]; % 状态转移矩阵
X_cv(:,1) = F_cv*[x0;y0;vx0;vy0] + W_cv; % 加过程噪声，也可不加
Z_cv = zeros(4,N); % 初始化目标测量值
V_cv = mvnrnd(zeros(1,4), R_matrix_cv)'; % 观测误差矩阵
H_cv = eye(4); % 状态观测矩阵
Z_cv(:,1) = H_cv*X_cv(:,1) + V_cv; % 测量值为真实值叠加观测误差
Xest_kf = zeros(4,N); % 卡尔曼滤波估计值
Xest_kf(:,1) = Z_cv(:,1); % 第一帧无法滤波，用测量值初始化
P_kf = zeros(4,4,N); % 状态估计协方差矩阵
P_kf(:,:,1) = eye(4); % 初始化状态估计协方差矩阵，常用单位矩阵
% 卡尔曼滤波核心算法
for i = 2:1:N
% 状态更新
X_cv(:,i) = F_cv*X_cv(:,i-1); % 通过状态转移矩阵计算下一时刻状态
% W_cv = mvnrnd(zeros(1,4), Q_matrix_cv)'; % 4维过程噪声向量
% X_cv(:,i) = X_cv(:,i) + W_cv; % 加过程噪声，也可不加
% 根据运动模型做下一时刻的预测
X_pre = F_cv*Xest_kf(:,i-1); % 状态预测
P_pre = F_cv*P_kf(:,:,i-1)*F_cv' + Q_matrix_cv; % 协方差矩阵预测
% 测量值更新
V_cv = mvnrnd(zeros(1,4), R_matrix_cv)'; % 观测误差矩阵
Z_cv(:,i) = H_cv*X_cv(:,i) + V_cv; % 测量值为真实值叠加观测误差
% 更新步骤
K_kf = P_pre*H_cv'/(H_cv*P_pre*H_cv' + R_matrix_cv); % 计算增益
Xest_kf(:,i) = X_pre + K_kf*(Z_cv(:,i) - H_cv*X_pre); % 计算状态估计
P_kf(:,:,i) = (eye(4) - K_kf*H_cv)*P_pre; % 计算协方差
end
% 绘图部分保持不变
figure;
size = 10;
width = 2;
% X位置曲线图
subplot(2,2,1);
plot(Z_cv(1,:),'.g','MarkerSize',size); hold on; % 画出测量值
plot(Xest_kf(1,:),'b','LineWidth',width);hold on; % 画出最优估计值
plot(X_cv(1,:),'r','LineWidth',width); % 画出实际状态值
title('X位置状态曲线');
legend('位置测量值', '位置最优估计值', '实际位置');
% Y距离曲线图
subplot(2,2,2);
plot(Z_cv(2,:),'.g','MarkerSize',size);hold on; % 画出测量值
plot(Xest_kf(2,:),'b','LineWidth',width);hold on; % 画出最优估计值
plot(X_cv(2,:),'r','LineWidth',width); % 画出实际状态值
title('Y位置状态曲线');
legend('位置测量值', '位置最优估计值', '实际位置');
% X速度曲线图
subplot(2,2,3);
plot(Z_cv(3,:),'.g','MarkerSize',size); hold on; % 画出测量值
plot(Xest_kf(3,:),'b','LineWidth',width); hold on; % 画出最优估计值
plot(X_cv(3,:),'r','LineWidth',width); % 画出实际状态值
title('X速度状态曲线');
legend('速度测量值', '速度最优估计值', '实际速度');
% Y速度曲线图
subplot(2,2,4);
plot(Z_cv(4,:),'.g','MarkerSize',size); hold on; % 画出测量值
plot(Xest_kf(4,:),'b','LineWidth',width); hold on; % 画出最优估计值
plot(X_cv(4,:),'r','LineWidth',width); % 画出实际状态值
title('Y速度状态曲线');
legend('速度测量值', '速度最优估计值', '实际速度');
% 位置平面图
% figure;
% plot(Z_cv(1,:),Z_cv(2,:));hold on; % 画出测量值
% plot(Xest_kf(1,:),Xest_kf(2,:)); % 画出最优估计值
% plot(X_cv(1,:),X_cv(2,:));hold on; % 画出实际值
% title('目标运动曲线');
% legend('位置测量值', '位置最优估计值', '实际位置');代码仿真了单个直线运动模型的卡尔曼滤波算法结果，如下图所示。
2 多传感器融合定位跟踪
如果是目标融合，需要两个目标做定位跟踪。这里假设摄像头目标、雷达目标交替出现，且符合线性运动模型。由于各自的噪声不一样，需要单独设置，然后用跟踪算法滤波做融合。
假设摄像头目标奇数帧出现，1,3,5,…,2n-1(n=1,2,3…N)，雷达目标偶数帧出现，2,4,6,…,2n(n=1,2,3…N)，然后分别生成目标并滤波，仿真代码如下。
% 匀速直线运动模型的卡尔曼滤波算法仿真
% 摄像头和雷达的测量值为x,y,vx,vy
clc;clear;close all;
% 匀速直线运动的初始值
x0 = 0; % 目标的初始横向位置
y0 = 0; % 目标的初始纵向位置
vx0 = 1; % 目标的横向速度
vy0 = 1; % 目标的纵向速度
N = 150; % 数据量
dt = 0.2; % 单帧时间
t = dt*(1:1:N); % 时间轴
% 设置过程噪声协方差矩阵Q，噪声来自真实世界中的不确定性，代表运动模型和实际运动的偏差
sigma_q_x = 0.1; % 纵向距离的过程噪声标准差
sigma_q_y = 0.1; % 横向距离的过程噪声标准差
sigma_q_vx = 0.1; % 纵向速度的过程噪声标准差
sigma_q_vy = 0.1; % 横向速度的过程噪声标准差
Q_matrix_cv = diag([sigma_q_x^2, sigma_q_y^2, sigma_q_vx^2, sigma_q_vy^2]);
% 设置测量噪声协方差矩阵R，噪声来自测量的误差
sigma_r_x = 1.0;
sigma_r_y = 0.2;
sigma_r_vx = 1.0;
sigma_r_vy = 0.2;
R_matrix_cv_camera = diag([sigma_r_x^2, sigma_r_y^2, sigma_r_vx^2, sigma_r_vy^2]);
% 上面是摄像头噪声协方差矩阵，下面是雷达噪声协方差矩阵
sigma_r_x = 0.5;
sigma_r_y = 0.5;
sigma_r_vx = 0.1;
sigma_r_vy = 0.1;
R_matrix_cv_radar = diag([sigma_r_x^2, sigma_r_y^2, sigma_r_vx^2, sigma_r_vy^2]);
% 卡尔曼滤波器初始化参数
X_cv = zeros(4,N); % 初始化目标运动真值
W_cv = mvnrnd(zeros(1,4), Q_matrix_cv)'; % 4维过程噪声向量
F_cv = [1 0 dt 0;
0 1 0 dt;
0 0 1 0;
0 0 0 1]; % 状态转移矩阵
X_cv(:,1) = F_cv*[x0;y0;vx0;vy0] + W_cv; % 加过程噪声，也可不加
Z_cv = zeros(4,N); % 初始化目标测量值
R_matrix_cv = R_matrix_cv_camera; % 第1帧为摄像头
V_cv = mvnrnd(zeros(1,4), R_matrix_cv)'; % 观测误差矩阵
H_cv = eye(4); % 状态观测矩阵
Z_cv(:,1) = H_cv*X_cv(:,1) + V_cv; % 测量值为真实值叠加观测误差
Xest_kf = zeros(4,N); % 卡尔曼滤波估计值
Xest_kf(:,1) = Z_cv(:,1); % 第一帧无法滤波，用测量值初始化
P_kf = zeros(4,4,N); % 状态估计协方差矩阵
P_kf(:,:,1) = eye(4); % 初始化状态估计协方差矩阵，常用单位矩阵
% 卡尔曼滤波核心算法
for i = 2:1:N
% 选择摄像头或雷达,摄像头是奇数帧，雷达是偶数帧
if mod(i,2) == 1
R_matrix_cv = R_matrix_cv_camera;
else
R_matrix_cv = R_matrix_cv_radar;
end
% 状态更新
X_cv(:,i) = F_cv*X_cv(:,i-1); % 通过状态转移矩阵计算下一时刻状态
% W_cv = mvnrnd(zeros(1,4), Q_matrix_cv)'; % 4维过程噪声向量
% X_cv(:,i) = X_cv(:,i) + W_cv; % 加过程噪声，也可不加
% 根据运动模型做下一时刻的预测
X_pre = F_cv*Xest_kf(:,i-1); % 状态预测
P_pre = F_cv*P_kf(:,:,i-1)*F_cv' + Q_matrix_cv; % 协方差矩阵预测
% 测量值更新
V_cv = mvnrnd(zeros(1,4), R_matrix_cv)'; % 观测误差矩阵
Z_cv(:,i) = H_cv*X_cv(:,i) + V_cv; % 测量值为真实值叠加观测误差
% 更新步骤
K_kf = P_pre*H_cv'/(H_cv*P_pre*H_cv' + R_matrix_cv); % 计算增益
Xest_kf(:,i) = X_pre + K_kf*(Z_cv(:,i) - H_cv*X_pre); % 计算状态估计
P_kf(:,:,i) = (eye(4) - K_kf*H_cv)*P_pre; % 计算协方差
end
% 绘图部分保持不变
figure;
size = 10;
width = 2;
% X位置曲线图
subplot(2,2,1);
plot(Z_cv(1,:),'.g','MarkerSize',size); hold on; % 画出测量值
plot(Xest_kf(1,:),'b','LineWidth',width);hold on; % 画出最优估计值
plot(X_cv(1,:),'r','LineWidth',width); % 画出实际状态值
title('X位置状态曲线');
legend('位置测量值', '位置最优估计值', '实际位置');
% Y距离曲线图
subplot(2,2,2);
plot(Z_cv(2,:),'.g','MarkerSize',size);hold on; % 画出测量值
plot(Xest_kf(2,:),'b','LineWidth',width);hold on; % 画出最优估计值
plot(X_cv(2,:),'r','LineWidth',width); % 画出实际状态值
title('Y位置状态曲线');
legend('位置测量值', '位置最优估计值', '实际位置');
% X速度曲线图
subplot(2,2,3);
plot(Z_cv(3,:),'.g','MarkerSize',size); hold on; % 画出测量值
plot(Xest_kf(3,:),'b','LineWidth',width); hold on; % 画出最优估计值
plot(X_cv(3,:),'r','LineWidth',width); % 画出实际状态值
title('X速度状态曲线');
legend('速度测量值', '速度最优估计值', '实际速度');
% Y速度曲线图
subplot(2,2,4);
plot(Z_cv(4,:),'.g','MarkerSize',size); hold on; % 画出测量值
plot(Xest_kf(4,:),'b','LineWidth',width); hold on; % 画出最优估计值
plot(X_cv(4,:),'r','LineWidth',width); % 画出实际状态值
title('Y速度状态曲线');
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_cv_camera = zeros(4,N_camera);
Z_cv_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_cv_camera(:,camera_count) = Z_cv(:,i); % 摄像头数据
else
radar_count = radar_count + 1;
radar_frame(radar_count) = i;
Z_cv_radar(:,radar_count) = Z_cv(:,i); % 雷达数据
end
end
% 绘图部分保持不变
figure;size = 10;width = 2;
% X位置曲线图
subplot(2,2,1);
plot(camera_frame,Z_cv_camera(1,:),'.g','MarkerSize',size); hold on; % 画出测量值
plot(radar_frame,Z_cv_radar(1,:),'.k','MarkerSize',size); hold on; % 画出测量值
plot(Xest_kf(1,:),'b','LineWidth',width);hold on; % 画出最优估计值
plot(X_cv(1,:),'r','LineWidth',width); % 画出实际状态值
title('X位置状态曲线');
legend('摄像头位置测量值', '雷达位置测量值','位置最优估计值', '实际位置');
% Y位置曲线图
subplot(2,2,2);
plot(camera_frame,Z_cv_camera(2,:),'.g','MarkerSize',size); hold on; % 画出测量值
plot(radar_frame,Z_cv_radar(2,:),'.k','MarkerSize',size); hold on; % 画出测量值
plot(Xest_kf(2,:),'b','LineWidth',width);hold on; % 画出最优估计值
plot(X_cv(2,:),'r','LineWidth',width); % 画出实际状态值
title('Y位置状态曲线');
legend('摄像头位置测量值', '雷达位置测量值','位置最优估计值', '实际位置');
% X速度曲线图
subplot(2,2,3);
plot(camera_frame,Z_cv_camera(3,:),'.g','MarkerSize',size); hold on; % 画出测量值
plot(radar_frame,Z_cv_radar(3,:),'.k','MarkerSize',size); hold on; % 画出测量值
plot(Xest_kf(3,:),'b','LineWidth',width); hold on; % 画出最优估计值
plot(X_cv(3,:),'r','LineWidth',width); % 画出实际状态值
title('X速度状态曲线');
legend('摄像头速度测量值', '雷达速度测量值', '速度最优估计值', '实际速度');
% Y速度曲线图
subplot(2,2,4);
plot(camera_frame,Z_cv_camera(4,:),'.g','MarkerSize',size); hold on; % 画出测量值
plot(radar_frame,Z_cv_radar(4,:),'.k','MarkerSize',size); hold on; % 画出测量值
plot(Xest_kf(4,:),'b','LineWidth',width); hold on; % 画出最优估计值
plot(X_cv(4,:),'r','LineWidth',width); % 画出实际状态值
title('Y速度状态曲线');
legend('摄像头速度测量值', '雷达速度测量值', '速度最优估计值', '实际速度');
% 位置平面图
% figure;
% plot(Z_cv(1,:),Z_cv(2,:));hold on; % 画出测量值
% plot(Xest_kf(1,:),Xest_kf(2,:)); % 画出最优估计值
% plot(X_cv(1,:),X_cv(2,:));hold on; % 画出实际值
% title('目标运动曲线');
% legend('位置测量值', '位置最优估计值', '实际位置');下面是两种传感器目标定位跟踪融合的结果，第一张图的测量值没有区分摄像头和雷达，第二张图做了区分。
