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

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


原理

卡尔曼滤波的细节可以参考下面这些,有直观解释也有数学推导。
运动目标跟踪(一)--搜索算法预测模型之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));

卡尔曼滤波效果演示

典型的一组测试结果为:
卡尔曼滤波之前的定位精度: 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/) 关于作者:目前主要研究领域为机器学习与无线定位技术,欢迎讨论与指正! 版权声明:本文版权归作者和博客园共有,转载请注明出处。
posted @ 2016-12-25 19:39  rubbninja  阅读(27607)  评论(6编辑  收藏