室內定位系列(五) 目標跟蹤(卡爾曼濾波)

2022-11-24 17:54:20 字數 4531 閱讀 2936

進行目標跟蹤時,先驗知識告訴我們定位軌跡是平滑的,目標當前時刻的狀態與上一時刻的狀態有關,濾波方法可以將這些先驗知識考慮進來得到更準確的定位軌跡。本文簡單介紹卡爾曼濾波及其使用。

初學者的卡爾曼濾波——擴充套件卡爾曼濾波(一)

理解kalman濾波的使用

這裡僅從目標定位跟蹤的角度做一個簡化版的介紹。

定位跟蹤時,可以通過某種定位技術(比如位置指紋法)得到一個位置估計(觀測位置),也可以根據我們的經驗(運動目標常常是勻速運動的)由上一時刻的位置和速度來**出當前位置(**位置)。把這個觀測結果和**結果做一個加權平均作為定位結果,權值的大小取決於觀測位置和**位置的不確定性程度,在數學上可以證明在**過程和觀測過程都是線性高斯時,按照卡爾曼的方法做加權是最優的。

擴充套件:在有些應用中,如果不是線性高斯的情況該怎麼辦?可以採用ekf(擴充套件卡爾曼濾波),在工作點附近對系統進行線性化,即使不是高斯也近似成高斯去做。這樣做有點太粗糙了,於是又有了iekf(迭代卡爾曼濾波,對工作點進行迭代優化),ukf或spkf(無跡卡爾曼濾波,不做線性化,而是投影做出一個高斯分佈去近似)。或者拋棄各種假設,直接採用蒙特卡洛的方式,假設出很多的粒子去近似分佈,就是pf(粒子濾波)。

理解下面這個五個方程的含義就可以了:

\[\hat_k^- = a\hat_ + bu_

\]\[p_k^- = a p_ a^t + q

\]\[k_k = p_k^- h^t (hp_k^-h^t + r)^

\]\[\hat_k = \hat_k^- + k_k (z_k - h\hat_k^-)

\]\[p_k = p_k^ - k_k h p_k^-

\]公式(步驟)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版參考

函式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); % 卡爾曼濾波

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

endkf_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,之所以能到達這樣好的結果,是因為充分使用了先驗知識:目標的運動是連續且基本勻速的。

出處:[