python多目标跟踪卡尔曼滤波_室内定位系列（五）——目标跟踪（卡尔曼滤波）...

进行目标跟踪时，先验知识告诉我们定位轨迹是平滑的，目标当前时刻的状态与上一时刻的状态有关，滤波方法可以将这些先验知识考虑进来得到更准确的定位轨迹。本文简单介绍卡尔曼滤波及其使用。

原理

这里仅从目标定位跟踪的角度做一个简化版的介绍。

定位跟踪时，可以通过某种定位技术(比如位置指纹法)得到一个位置估计(观测位置)，也可以根据我们的经验(运动目标常常是匀速运动的)由上一时刻的位置和速度来预测出当前位置(预测位置)。把这个观测结果和预测结果做一个加权平均作为定位结果，权值的大小取决于观测位置和预测位置的不确定性程度，在数学上可以证明在预测过程和观测过程都是线性高斯时，按照卡尔曼的方法做加权是最优的。

扩展：在有些应用中，如果不是线性高斯的情况该怎么办？可以采用EKF(扩展卡尔曼滤波)，在工作点附近对系统进行线性化，即使不是高斯也近似成高斯去做。这样做有点太粗糙了，于是又有了IEKF(迭代卡尔曼滤波，对工作点进行迭代优化)，UKF或SPKF(无迹卡尔曼滤波，不做线性化，而是投影做出一个高斯分布去近似)。或者抛弃各种假设，直接采用蒙特卡洛的方式，假设出很多的粒子去近似分布，就是PF(粒子滤波)。

步骤

理解下面这个五个方程的含义就可以了：

\[\hat{x}_k^- = A\hat{x}_{k-1} + Bu_{k-1}$$$$P_k^- = A P_{k-1} A^T + Q$$$$K_k = P_k^- H^T (HP_k^-H^T + R)^{-1}$$$$\hat{x}_k = \hat{x}_k^- + K_k (z_k - H\hat{x}_k^-)$$$$P_k = P_k^{-} - K_k H P_k^-

\]

公式(步骤)1：由上一时刻的状态预测当前状态，加上外界的输入。

公式(步骤)2：预测过程增加了新的不确定性\(Q\)，加上之前存在的不确定性。

公式(步骤)3：由预测结果的不确定性\(P_k^-\)和观测结果的不确定性\(R\)计算卡尔曼增益(权重)。

公式(步骤)4：对预测结果和观测结果做加权平均，得到当前时刻的状态估计。

公式(步骤)5：更新\(P_k\)，代表本次状态估计的不确定性。

需要注意的是，在定位中状态\(x_k\)是一个向量，除了坐标外还可以包含速度，比如\(x_k = (坐标x，坐标y，速度x，速度y)\)，状态是向量而不仅仅是一个标量，上面的几个公式中的矩阵乘法实际上是同时对多个状态进行计算，表示不确定性的方差也就成了协方差矩阵。

实践

下面用matlab动手写一个卡尔曼滤波：首次使用卡尔曼滤波时先调用函数kf_init()对初始化结构体kf_params的各项参数，之后每次滤波时，设置当前的观测值，调用kf_update()进行更新，定位结果包含在返回的参数kf_params中。

Github地址

python版参考http://www.cnblogs.com/rubbninja/p/6256072.html

函数kf_update()

function kf_params = kf_update(kf_params)

% 以下为卡尔曼滤波的五个方程(步骤)

x_ = kf_params.A * kf_params.x + kf_params.B * kf_params.u;

P_ = kf_params.A * kf_params.P * kf_params.A' + kf_params.Q;

kf_params.K = P_ * kf_params.H' * (kf_params.H * P_ * kf_params.H' + kf_params.R)^-1;

kf_params.x = x_ + kf_params.K * (kf_params.z - kf_params.H * x_);

kf_params.P = P_ - kf_params.K * kf_params.H * P_;

end

函数kf_init()

function kf_params = kf_init(Px, Py, Vx, Vy)

%% 本例中，状态x为(坐标x， 坐标y， 速度x， 速度y)，观测值z为(坐标x， 坐标y)

kf_params.B = 0; %外部输入为0

kf_params.u = 0; %外部输入为0

kf_params.K = NaN; %卡尔曼增益无需初始化

kf_params.z = NaN; %这里无需初始化，每次使用kf_update之前需要输入观察值z

kf_params.P = zeros(4, 4); %初始P设为0

%% 初始状态：函数外部提供初始化的状态，本例使用观察值进行初始化，Vx，Vy初始为0

kf_params.x = [Px; Py; Vx; Vy];

%% 状态转移矩阵A

kf_params.A = eye(4) + diag(ones(1, 2), 2); % 和线性系统的预测机制有关，这里的线性系统是上一刻的位置加上速度等于当前时刻的位置，而速度本身保持不变

%% 预测噪声协方差矩阵Q：假设预测过程上叠加一个高斯噪声，协方差矩阵为Q

%大小取决于对预测过程的信任程度。比如，假设认为运动目标在y轴上的速度可能不匀速，那么可以把这个对角矩阵的最后一个值调大。有时希望出来的轨迹更平滑，可以把这个调更小

kf_params.Q = diag(ones(4, 1) * 0.001);

%% 观测矩阵H：z = H * x

kf_params.H = eye(2, 4); % 这里的状态是(坐标x， 坐标y， 速度x， 速度y)，观察值是(坐标x， 坐标y)，所以H = eye(2, 4)

%% 观测噪声协方差矩阵R：假设观测过程上存在一个高斯噪声，协方差矩阵为R

kf_params.R = diag(ones(2, 1) * 2); %大小取决于对观察过程的信任程度。比如，假设观测结果中的坐标x值常常很准确，那么矩阵R的第一个值应该比较小

end

测试卡尔曼滤波的效果

模拟一条运动轨迹，然后加上高斯观察噪声，作为观测位置轨迹。然后使用卡尔曼滤波得到滤波后的结果。可以分别计算出观察位置轨迹的定位精度和滤波后轨迹的定位精度。

addpath('./filters');

addpath('./IP_raytracing');

%% 模拟一条运动轨迹，然后加上高斯观察噪声，作为观测位置轨迹。然后使用卡尔曼滤波得到滤波后的结果。

% 速度为均值0.6m标准差0.05的高斯分布

% 观测噪声标准差为2

%% 画出实际的真实路径

roomLength = 1000;

roomWidth = 1000;

t = 500;

trace_real = get_random_trace(roomLength, roomWidth, t);

figure;

subplot(1, 3, 1); plot(trace_real(:, 1), trace_real(:, 2), '.');

title('实际的真实路径');

%% 有观测噪声时的路径

noise = 2; %2m的位置波动噪声

trace = trace_real + normrnd(0, noise, size(trace_real));

subplot(1, 3, 2); plot(trace(:, 1), trace(:, 2), '.');

title('有噪声时的路径');

fprintf('卡尔曼滤波之前的定位精度： %f m\n', accuracy(trace, trace_real));

%% 对有噪声的路径进行卡尔曼滤波

kf_params_record = zeros(size(trace, 1), 4);

for i = 1 : t

if i == 1

kf_params = kf_init(trace(i, 1), trace(i, 2), 0, 0); % 初始化

else

kf_params.z = trace(i, 1:2)'; %设置当前时刻的观测位置

kf_params = kf_update(kf_params); % 卡尔曼滤波

end

kf_params_record(i, :) = kf_params.x';

end

kf_trace = kf_params_record(:, 1:2);

subplot(1, 3, 3); plot(kf_trace(:, 1), kf_trace(:, 2), '.');

title('卡尔曼滤波后的效果');

fprintf('卡尔曼滤波之后的定位精度： %f m\n', accuracy(kf_trace, trace_real));

典型的一组测试结果为：

卡尔曼滤波之前的定位精度： 2.424880 m

卡尔曼滤波之后的定位精度： 1.426890 m

在这组测试中，滤波后的轨迹更平滑，而且精度从2.4m提高到1.4m，之所以能到达这样好的结果，是因为充分使用了先验知识：目标的运动是连续且基本匀速的。

作者：[rubbninja](http://www.cnblogs.com/rubbninja/)

出处：[http://www.cnblogs.com/rubbninja/](http://www.cnblogs.com/rubbninja/)

关于作者：目前主要研究领域为机器学习与无线定位技术，欢迎讨论与指正！

版权声明：本文版权归作者和博客园共有，转载请注明出处。