目标跟踪（最全最详细的卡尔曼滤波法：原理+代码）【西电电子信息综合实验四】
目标跟踪（最全最详细的卡尔曼滤波法：原理+代码）【西电电子信息综合实验四】
卡尔曼滤波法是一种在工程和科学领域广泛应用的线性滤波技术，特别适用于目标跟踪等动态系统状态估计问题。本文将从原理到实践，全面介绍卡尔曼滤波法在目标跟踪中的应用，包括状态方程、量测方程、算法流程以及MATLAB代码实现。
一.状态方程描述
若讨论二维匀速模型，可以非常容易地得出位置状态转移的方程：
也可以非常容易地得到速度状态转移的方程：
如果讨论过程噪声：就是目标不可能做绝对匀速运动，其速度必然有一些小的随机波动，例如目标在匀速运动过程中，驾驶员或环境扰动等都可造成速度出现不可预测的变化，像飞机飞行过程中云层和阵风对飞机飞行速度的影响等，而这些速度的小的变化可看作过程噪声来建模。
那么加上过程噪声之后的位置状态转移方程为：
加上过程噪声之后的速度状态转移方程为：
用矩阵的形式表达为：
有状态转移方程：
可以得到状态转移矩阵 :F(K)
按照教案，只考虑零均值的高斯白噪声模型为：
=
那么有状态转移方程：
二.量测方程：是雷达测量过程的假设，对于线性系统而言量测方程可表示为
符号解释：Z（k+1）为量测向量；H（k+1）为量测矩阵；W（k+1）为具有协方差 R（k+1）的零均值、白色高斯量测噪声序列
又因为状态向量为：
量测向量为：
因此为了满足量测方程，有量测矩阵H(X)：
三.卡尔曼滤波方法
根据最小均方误差推导得出
通过卡尔曼算法，通过新息与卡尔曼增益的乘积加上状态的第一步预测，得到状态的最终预测值
这里的新息是量测的估计值和量测的预测值的差值
卡尔曼增益由状态向量和量测向量的协方差矩阵构成
这一段内容很难看懂，不过，本质上，就是通过量测和状态，预测出下一个点的状态向量，接下来会详细解释其过程
具体步骤如下：
1.状态的第一步预测：
上面的G(K)*U(K)是控制量这里不予讨论，忽略得到状态第一步预测的简化方程
2.量测的预测值
3.状态值误差
由状态值的估计值减去状态预测值的第一步预测值得到
4.衡量预测协方差矩阵
这里的 Q矩阵是合成噪声矩阵，我们假设各噪声相互独立，且都是零均值的高斯白噪声，那么该矩阵为单位矩阵，所以后面的噪声合成矩阵我们都假定为单位矩阵。
这里用估计值减去预测值，再衡量协方差，其意义是通过协方差矩阵衡量预测的不确定性
协方差越小，预测准确性越大，不确定性越小。
5.量测值误差（新息）
由量测值的估计值减去量测预测值的第一步预测值得到
也可以用v（k+1）表示新息
6.衡量量测协方差矩阵（新息协方差矩阵）
同理可得
同理，其也能衡量新息的不确定性，协方差越小，预测准确性越大，不确定性越小
7.状态与量测之间的协方差矩阵
8.衡量最小均方误差所得到的预测值
由
有
由上下恒等式可以推出卡尔曼增益：
这样就能计算出卡尔曼增益，也就能通过量测和状态量预测出下一个状态量了！
9.协方差更新方程为
这里相减的理解是融合克尔曼增量到新的协方差中，这里的推导可以理解为在更新状态后，如何将新观测的不确定性融入到状态协方差中。通过K k K_kKk 的引入，我们能够调整协方差，使其反映观测对状态估计的不确定性影响。（说实话这一步不是很懂，能用就行）
整理，简化之后，可以得到以下算法流程
四.算法流程
- 初始设定
在卡尔曼滤波中，状态模型可以表示为：
x k = F x k − 1 + w k x_k = F x_{k-1} + w_kxk =Fxk−1 +wk
其中：
(x_k) 是时刻 (k) 的状态向量。
(F) 是状态转移矩阵。
(w_k) 是过程噪声，通常假设为均值为零的高斯噪声，协方差为 (Q)。
观测模型为：
z k = H x k + v k z_k = H x_k + v_kzk =Hxk +vk
其中：
(z_k) 是观测向量。
(H) 是观测矩阵。
(v_k) 是观测噪声，通常假设为均值为零的高斯噪声，协方差为 (R)。
- 预测步骤
在预测步骤中，基于上一个时刻的状态估计 (x_{k-1|k-1}) 和协方差 (P_{k-1|k-1})，预测状态和协方差为：
x k ∣ k − 1 = F x k − 1 ∣ k − 1 x_{k|k-1} = F x_{k-1|k-1}xk∣k−1 =Fxk−1∣k−1
P k ∣ k − 1 = F P k − 1 ∣ k − 1 F T + Q P_{k|k-1} = F P_{k-1|k-1} F^T + QPk∣k−1 =FPk−1∣k−1 FT+Q
这里，(P_{k|k-1}) 的推导考虑了状态转移对不确定性的影响，并加入了过程噪声 (Q)。
- 更新步骤
在更新步骤中，我们使用观测 (z_k) 来调整状态估计和协方差。首先计算新息（残差）：
y k = z k − H x k ∣ k − 1 y_k = z_k - H x_{k|k-1}yk =zk −Hxk∣k−1
然后，计算新息的协方差（即残差协方差）：
S k = H P k ∣ k − 1 H T + R S_k = H P_{k|k-1} H^T + RSk =HPk∣k−1 HT+R
- 卡尔曼增益
卡尔曼增益 (K_k) 是为了优化状态估计而计算的：
K k = P k ∣ k − 1 H K_k = P_{k|k-1} HKk =Pk∣k−1 H T S k T S_kTSk − 1 {-1}−1
- 更新状态估计和协方差
状态估计更新为：
x k ∣ k = x k ∣ k − 1 + K k y k x_{k|k} = x_{k|k-1} + K_k y_kxk∣k =xk∣k−1 +Kk yk
6.接下来推导协方差更新方程：
P k ∣ k = ( I − K k H ) P k ∣ k − 1 P_{k|k} = (I - K_k H) P_{k|k-1}Pk∣k =(I−Kk H)Pk∣k−1
这里的推导可以理解为在更新状态后，如何将新观测的不确定性融入到状态协方差中。通过 (K_k) 的引入，我们能够调整协方差，使其反映观测对状态估计的不确定性影响。
7.流程图总结
协方差更新方程的推导涉及到状态预测、观测更新及卡尔曼增益的计算。通过这些步骤，我们能够得到更新后的状态协方差，确保状态估计能够随着新的观测数据不断优化。
卡尔曼滤波器不仅是所有线性滤波器中最好的滤波器，而且当过程噪声为高斯过程时，它是所有滤波器中最好的滤波器。卡尔曼滤波除了系统噪声和量测噪声为高斯白噪声且已知其二阶矩之外，不需任何其他条件，因而完全适用于非平稳、多维的随机序列的估计问题
五.代码
下面是用加高斯白噪声的匀速直线模型，克尔曼滤波法目标跟踪 vs 粒子滤波目标跟踪的代码
% 清空工作空间
clear; clc; close all;
% 参数设置
dt = 1; % 时间间隔
numSteps = 500; % 总步骤数
numParticles = 100; % 粒子数量
% 目标的真实状态（位置和速度）
trueState = [0; 0; 1; 1]; % [x; y; vx; vy]
% 生成真实轨迹
trajectory = zeros(4, numSteps);
for k = 1:numSteps
trueState(1:2) = trueState(1:2) + trueState(3:4) * dt+randn(2,1); % 更新位置
trajectory(:, k) = trueState;
end
% 生成观测数据（带有高斯噪声）
observations = trajectory(1:2, :) + randn(2, numSteps) * 2;
% 卡尔曼滤波器
% 初始化状态和协方差
x_kf = [0; 0; 0; 0]; % 初始估计
P_kf = eye(4); % 初始协方差
% 状态转移矩阵和观测矩阵
F = [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1];
H = [1 0 0 0; 0 1 0 0];
% 存储卡尔曼滤波结果
kfEstimate = zeros(4, numSteps);
for k = 1:numSteps
% 预测
x_kf = F * x_kf+[randn(2,1);0;0]; % 预测状态
P_kf = F * P_kf * F' + eye(4); % 预测协方差
% 更新
z_kf = observations(:, k); % 当前观测
y_kf = z_kf - H * x_kf; % 计算残差
S_kf = H * P_kf * H' + eye(2) * 4; % 残差协方差
K_kf = P_kf * H' / S_kf; % 卡尔曼增益
x_kf = x_kf + K_kf * y_kf; % 更新状态
P_kf = (eye(4) - K_kf * H) * P_kf; % 更新协方差
kfEstimate(:, k) = x_kf; % 存储估计
end
% 粒子滤波器
% 初始化粒子
particles = [randn(2, numParticles) * 20; randn(2, numParticles)];
weights = ones(numParticles, 1) / numParticles;
% 存储粒子滤波结果
pfEstimate = zeros(2, numSteps);
for k = 1:numSteps
% 预测步骤
particles(1:2, :) = particles(1:2, :) + particles(3:4, :) * dt + randn(2, numParticles);
% 更新步骤
distances = sqrt(sum((particles(1:2, :) - observations(:, k)).^2));
weights = exp(-distances.^2 / (2 * 4^2)); % 观测权重
weights = weights / sum(weights); % 归一化
% 计算估计
pfEstimate(:, k) = sum(particles(1:2, :) .* weights, 2);
% 重采样
indices = resample(weights, numParticles);
particles = particles(:, indices);
end
% 绘制结果
figure;
plot(trajectory(1, :), trajectory(2, :), 'g-', 'DisplayName', '真实轨迹');
hold on;
plot(observations(1, :), observations(2, :), 'ro', 'DisplayName', '观测数据');
plot(kfEstimate(1, :), kfEstimate(2, :), 'b-', 'DisplayName', '卡尔曼滤波估计');
plot(pfEstimate(1, :), pfEstimate(2, :), 'm-', 'DisplayName', '粒子滤波估计');
legend;
xlabel('X位置');
ylabel('Y位置');
title('目标检测：卡尔曼滤波 vs 粒子滤波');
%grid on;
function indices = resample(weights, numParticles)
cumulativeSum = cumsum(weights);
randomSamples = rand(numParticles, 1);
indices = zeros(numParticles, 1);
for i = 1:numParticles
indices(i) = find(randomSamples(i) <= cumulativeSum, 1, 'first');
end
end
其中位置200-300的放大图片
3维目标跟踪
只需要更换状态变量，多一个z轴的位置状态和z轴的速度状态
更换状态转移矩阵为
更换量测矩阵为
就能做出三维的目标跟踪了！
其中放大截取一部分，可以发现，卡尔曼滤波跟踪效果在线性上要强于粒子滤波
六.完结感悟
这一次的对于卡尔曼滤波法实现目标跟踪的探索，对于研究者而言具有里程碑意义。通过深入探索这一具有复杂原理的算法，可以为未来研究更多复杂算法提供宝贵经验。研究这类算法时，可以从输入输出关系入手，理解算法本质原理和模型建立，明确假设条件，并将原理串联起来，融汇贯通。
祝大家在实验室玩的开心！
七.参考文献
1.雷达数据处理与应用 何友等著
2.chatgpt