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

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

原理

卡尔曼滤波的细节可以参考下面这些，有直观解释也有数学推导。
运动目标跟踪（一）--搜索算法预测模型之KF,EKF,UKF
初学者的卡尔曼滤波——扩展卡尔曼滤波（一）
理解Kalman滤波的使用

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

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

扩展：在有些应用中，如果不是线性高斯的情况该怎么办？可以采用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));