线性卡尔曼跟踪融合滤波算法（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('位置测量值', '位置最优估计值', '实际位置');

? ? ? ? 下面是两种传感器目标定位跟踪融合的结果，第一张图的测量值没有区分摄像头和雷达，第二张图做了区分。