您當(dāng)前位置：首頁(yè) > 互聯(lián)網(wǎng) > 卡爾曼濾波器算法淺析及matlab實(shí)戰(zhàn)

卡爾曼濾波器算法淺析及matlab實(shí)戰(zhàn)

來(lái)源：程序員人生發(fā)布時(shí)間：2015-05-12 08:58:49 閱讀次數(shù)：6476次

卡爾曼濾波器是1種利用線性系統(tǒng)狀態(tài)方程，通過(guò)系統(tǒng)輸入輸出觀測(cè)數(shù)據(jù)，對(duì)系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)的算法。而且由于觀測(cè)包括系統(tǒng)的噪聲和干擾的影響，所以最優(yōu)估計(jì)也可看作是濾波進(jìn)程。

卡爾曼濾波器的核心內(nèi)容就是5條公式，計(jì)算簡(jiǎn)單快速，合適用于少許數(shù)據(jù)的預(yù)測(cè)和估計(jì)。

下面我們用1個(gè)例子來(lái)講明1下卡爾曼算法的利用。

假定我們想在有1輛小車，在 t 時(shí)刻其速度為 Vt ，位置坐標(biāo)為 Pt，ut 表示 t 時(shí)刻的加速度，那末我們可以用Xt表示 t 時(shí)刻的狀態(tài)，以下：

則我們可以得到，由t⑴ 時(shí)刻到 t 時(shí)刻，位置和速度的轉(zhuǎn)換以下：

用向量表示上述轉(zhuǎn)換進(jìn)程，以下：

以下圖：

那末我們可以得到以下的狀態(tài)轉(zhuǎn)移公式：

(1)

其中矩陣 F 為狀態(tài)轉(zhuǎn)移矩陣，表示如何從上1狀態(tài)來(lái)推測(cè)當(dāng)前時(shí)刻的狀態(tài)，B 為控制矩陣，表示控制量u如何作用于當(dāng)前矩陣，上面的公式 x 有頂帽子，表示只是估計(jì)值，其實(shí)不是最優(yōu)的。

有了狀態(tài)轉(zhuǎn)移公式就能夠用來(lái)推測(cè)當(dāng)前的狀態(tài)，但是所有的推測(cè)都是包括噪聲的，噪聲越大，不肯定越大，協(xié)方差矩陣用來(lái)表示這次推測(cè)帶來(lái)的不肯定性

協(xié)方差矩陣

假定我們有1個(gè)1維的數(shù)據(jù)，這個(gè)數(shù)據(jù)每次丈量都不同，我們假定服從高斯散布，那末我們可以用均值和方差來(lái)表示該數(shù)據(jù)集，我們將該1維數(shù)據(jù)集投影到坐標(biāo)軸上，以下圖：

可以看到，服從高斯散布的1維數(shù)據(jù)大部份散布在均值附近。

現(xiàn)在我們來(lái)看看服從高斯散布的2維數(shù)據(jù)投影到坐標(biāo)軸的情況，以下圖：

2維數(shù)據(jù)比1維數(shù)據(jù)略微復(fù)雜1點(diǎn)，投影后有3種情況，分別是：
左圖：兩個(gè)維的數(shù)據(jù)互不相干；
中圖：兩個(gè)維的數(shù)據(jù)正相干，也就是 y 隨著 x 的增大而增大（假定兩個(gè)維分別為 x 和 y）
右圖：兩個(gè)維的數(shù)據(jù)負(fù)相干，也就是 y 隨著 x 的增大而減小。

那怎樣來(lái)表示兩個(gè)維的數(shù)據(jù)的相干性呢？答案就是協(xié)方差矩陣。

狀態(tài)協(xié)方差矩陣傳遞

在公式（1）當(dāng)中，我們已得到了狀態(tài)的轉(zhuǎn)移公式，但是由上面可知，2維數(shù)據(jù)的協(xié)方差矩陣對(duì)描寫數(shù)據(jù)的特點(diǎn)是很重要的，那末我們應(yīng)當(dāng)如何更新或說(shuō)傳遞我們的2維數(shù)據(jù)的協(xié)方差矩陣呢？假設(shè)我們用 P 來(lái)表示狀態(tài)協(xié)方差，即

那末加入狀態(tài)轉(zhuǎn)換矩陣 F ，得到

(2)

也即：

因此我們便得到了協(xié)方差的轉(zhuǎn)換公式。

現(xiàn)在我們得到了兩個(gè)公式，應(yīng)用這兩個(gè)公式能夠?qū)ΜF(xiàn)在狀態(tài)進(jìn)行預(yù)測(cè)。依照我們的正常思路來(lái)理解，預(yù)測(cè)結(jié)果不1定會(huì)對(duì)嘛，肯定有誤差。而且在我們大多數(shù)回歸算法或是擬合算法中，1般思路都是先預(yù)測(cè)，然后看看這個(gè)預(yù)測(cè)結(jié)果跟實(shí)際結(jié)果的誤差有多大，再根據(jù)這個(gè)誤差來(lái)調(diào)劑預(yù)測(cè)函數(shù)的參數(shù)，不斷迭代調(diào)劑參數(shù)直到預(yù)測(cè)誤差小于1定的閾值。

卡爾曼算法的迭代思想也類似，不過(guò)這里根據(jù)誤差調(diào)劑的是狀態(tài) X 。

在這里，我們的實(shí)際數(shù)據(jù)就是 Z，以下圖：

其中，矩陣 H 為丈量系統(tǒng)的參數(shù)，即視察矩陣，v 為觀測(cè)噪聲，其協(xié)方差矩陣為R

那末我們的狀態(tài)更新公式以下：

其中K 為卡爾曼系數(shù)， Z-Hx 則為殘差，也就是我們說(shuō)的，預(yù)測(cè)值與實(shí)際值的誤差。

K的作用：

1.K 權(quán)衡預(yù)測(cè)協(xié)方差P和視察協(xié)方差矩陣R那個(gè)更加重要，相信預(yù)測(cè)，殘差的權(quán)重小，相信視察，殘差權(quán)重大，由 K 的表達(dá)是可以退出這個(gè)結(jié)論
2,將殘差的表現(xiàn)情勢(shì)從視察域轉(zhuǎn)換到狀態(tài)域（殘差與1個(gè)標(biāo)量，通過(guò)K 轉(zhuǎn)換為向量），由狀態(tài) X 的更新公式可得到該結(jié)論。

至此，我們已得到了 t 狀態(tài)下的最優(yōu)估計(jì)值 Xt。但為了能讓我們的迭代算法延續(xù)下去，我們還必須更新狀態(tài)協(xié)方差的值。

狀態(tài)協(xié)方差的更新

以上就是卡爾曼濾波算法的思想，只有簡(jiǎn)單的 5 條公式，總結(jié)以下：

Matlab 實(shí)現(xiàn)

function kalmanFiltering
%%
clc
close all

%%
%
%   Description : kalmanFiltering
%   Author : Liulongpo
%   Time：2015⑷⑵9 16:42:34
%

%%
Z=(1:2:200); %觀測(cè)值  汽車的位置  也就是我們要修改的量
noise=randn(1,100); %方差為1的高斯噪聲
Z=Z+noise;
X=[0 ; 0 ]; %初始狀態(tài)
P=[1 0;0 1]; %狀態(tài)協(xié)方差矩陣
F=[1 1;0 1]; %狀態(tài)轉(zhuǎn)移矩陣
Q=[0.0001,0;0 , 0.0001]; %狀態(tài)轉(zhuǎn)移協(xié)方差矩陣
H=[1,0]; %觀測(cè)矩陣
R=1; %觀測(cè)噪聲方差
figure;
hold on;
for i = 1:100
%基于上1狀態(tài)預(yù)測(cè)當(dāng)前狀態(tài)  
X_ = F*X;
% 更新協(xié)方差  Q系統(tǒng)進(jìn)程的協(xié)方差  這兩個(gè)公式是對(duì)系統(tǒng)的預(yù)測(cè)
P_ = F*P*F'+Q;
% 計(jì)算卡爾曼增益
K = P_*H'/(H*P_*H'+R);
% 得到當(dāng)前狀態(tài)的最優(yōu)化估算值  增益乘以殘差
X = X_+K*(Z(i)-H*X_);
%更新K狀態(tài)的協(xié)方差
P = (eye(2)-K*H)*P_;
scatter(X(1), X(2),4); %畫點(diǎn)，橫軸表示位置，縱軸表示速度
end

end

效果以下

其中 x 軸為位置，y軸為速度。
在代碼中，我們?cè)O(shè)定x的變化是 1:2:200，則速度就是2，可以由上圖看到，值經(jīng)過(guò)幾次迭代，速度就基本上在 2 附近擺動(dòng)，擺動(dòng)的緣由是我們加入了噪聲。

接下來(lái)來(lái)看1個(gè)實(shí)際例子。

我們的數(shù)據(jù)為 data = [149.360 , 150.06, 151.44, 152.81,154.19 ,157.72];
這是應(yīng)用光流法從視頻中獲得角點(diǎn)的實(shí)際x軸坐標(biāo)，總共有6個(gè)數(shù)據(jù)，也就是代表了1個(gè)點(diǎn)的連續(xù)6幀的x軸坐標(biāo)。接下來(lái)這個(gè)例子，我們將實(shí)現(xiàn)用5幀的數(shù)據(jù)進(jìn)行訓(xùn)練，然后預(yù)測(cè)出第6幀的x軸坐標(biāo)。

在上1個(gè)matlab例子中，我們的訓(xùn)練數(shù)據(jù)比較多，因此我們的初始狀態(tài)設(shè)置為[0,0]，也就是位置為0，速度為0，在訓(xùn)練數(shù)據(jù)比較多的情況下，初始化數(shù)據(jù)為0并沒(méi)有關(guān)系，由于我們?cè)谏厦娴男Ч麍D中可以看到，算法的經(jīng)太短暫的迭代就可以夠發(fā)揮作用。

但在這里，我們的訓(xùn)練數(shù)據(jù)只有5幀，所以為了加快訓(xùn)練，我們將位置狀態(tài)初始化為第1幀的位置，速度初始化為第2幀與第1幀之差。

代碼以下：

KF.m

function [predData,dataX] = KF(dataZ)

%%
%
%   Description : kalmanFiltering
%   Author : Liulongpo
%   Time：2015⑷⑵9 16:42:34
%
%%
Z = dataZ';
len = length(Z);
%Z=(1:2:200); %觀測(cè)值  汽車的位置  也就是我們要修改的量
noise=randn(1,len); %方差為1的高斯噪聲
dataX = zeros(2,len);
Z=Z+noise;
X=[Z(1) ; Z(2)-Z(1) ]; %初始狀態(tài)  分別為 位置 和速度
P=[1 0;0 1]; %狀態(tài)協(xié)方差矩陣
F=[1 1;0 1]; %狀態(tài)轉(zhuǎn)移矩陣
Q=[0.0001,0;0 , 0.0001]; %狀態(tài)轉(zhuǎn)移協(xié)方差矩陣
H=[1,0]; %觀測(cè)矩陣
R=1; %觀測(cè)噪聲方差
%figure;
%hold on;
for i = 1:len
%基于上1狀態(tài)預(yù)測(cè)當(dāng)前狀態(tài)  
% 2x1  2x1
X_ = F*X;
% 更新協(xié)方差  Q系統(tǒng)進(jìn)程的協(xié)方差  這兩個(gè)公式是對(duì)系統(tǒng)的預(yù)測(cè)
%   2x1  2x1  1x2  2x2
P_ = F*P*F'+Q;
% 計(jì)算卡爾曼增益
K = P_*H'/(H*P_*H'+R);
% 得到當(dāng)前狀態(tài)的最優(yōu)化估算值  增益乘以殘差
X = X_+K*(Z(i)-H*X_);
%更新K狀態(tài)的協(xié)方差
P = (eye(2)-K*H)*P_;
dataX(:,i) = [X(1);X(2)];
%scatter(X(1), X(2),4); %畫點(diǎn)，橫軸表示位置，縱軸表示速度
end
predData = F*X;
end

testKF.m

function testKF
%% 
clc
close all
%%
%data = load('D:a.txt');
%data = [149.360 , 150.06, 151.44, 152.81,154.19,157.72,157.47,159.33,153.66];
data = [149.360 , 150.06, 151.44, 152.81,154.19 ,157.72];
[predData , DataX] = KF(data');
error = DataX(1,:) - data;

i = 1:length(data);
figure
subplot 311
scatter(i,data,3),title('原始數(shù)據(jù)')
subplot 312
scatter(i,DataX(1,:),3),title('預(yù)測(cè)數(shù)據(jù)')
subplot 313
scatter(i,error,3),title('預(yù)測(cè)誤差')
predData(1)
%{
scatter(i,error,3);
figure
scatter(i,data,3)
figure
scatter(i,predData(1,:),3)
%}
end

效果以下：

預(yù)測(cè)結(jié)果為： 155.7493 ，跟實(shí)際結(jié)果 157.72 唯一1.9 的誤差，可以看到，卡爾曼濾波器算法對(duì)少許數(shù)據(jù)的預(yù)測(cè)效果還是挺不錯(cuò)的。固然，預(yù)測(cè)位置的同時(shí)，我們也得到了預(yù)測(cè)速度。

參考文獻(xiàn)：視頻: 卡爾曼濾波的原理和在MATLAB中的實(shí)現(xiàn)

生活不易，碼農(nóng)辛苦
如果您覺得本網(wǎng)站對(duì)您的學(xué)習(xí)有所幫助,可以手機(jī)掃描二維碼進(jìn)行捐贈(zèng)
程序員人生