原標(biāo)題：卡爾曼濾波器算法淺析及matlab實(shí)戰(zhàn)

作者：Liu_LongPo

出處：Liu_LongPo的博客

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

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

下面我們用一個例子來說明一下卡爾曼算法的應(yīng)用。

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

則我們可以得到，由t-1 時刻到 t 時刻，位置以及速度的轉(zhuǎn)換如下：

用向量表示上述轉(zhuǎn)換過程，如下：

如下圖：

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

(1)

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

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

協(xié)方差矩陣

假設(shè)我們有一個一維的數(shù)據(jù)，這個數(shù)據(jù)每次測量都不同，我們假設(shè)服從高斯分布，那么我們可以用均值和方差來表示該數(shù)據(jù)集，我們將該一維數(shù)據(jù)集投影到坐標(biāo)軸上，如下圖：

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

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

二維數(shù)據(jù)比一維數(shù)據(jù)稍微復(fù)雜一點(diǎn)，投影后有3種情況，分別是：

左圖：兩個維的數(shù)據(jù)互不相關(guān)；

中圖：兩個維的數(shù)據(jù)正相關(guān)，也就是 y 隨著 x 的增大而增大(假設(shè)兩個維分別為 x 和 y)

右圖：兩個維的數(shù)據(jù)負(fù)相關(guān)，也就是 y 隨著 x 的增大而減小。

那怎么來表示兩個維的數(shù)據(jù)的相關(guān)性呢？答案就是協(xié)方差矩陣。

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

在公式(1)之中，我們已經(jīng)得到了狀態(tài)的轉(zhuǎn)移公式，但是由上面可知，二維數(shù)據(jù)的協(xié)方差矩陣對于描述數(shù)據(jù)的特征是很重要的，那么我們應(yīng)該如何更新或者說傳遞我們的二維數(shù)據(jù)的協(xié)方差矩陣呢？假如我們用 P 來表示狀態(tài)協(xié)方差，即

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

(2)

也即：

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

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

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

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

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

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

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

K的作用：

1.K 權(quán)衡預(yù)測協(xié)方差P和觀察協(xié)方差矩陣R那個更加重要，相信預(yù)測，殘差的權(quán)重小，相信觀察，殘差權(quán)重大，由 K 的表達(dá)是可以退出這個結(jié)論

2,將殘差的表現(xiàn)形式從觀察域轉(zhuǎn)換到狀態(tài)域(殘差與一個標(biāo)量，通過K 轉(zhuǎn)換為向量)，由 狀態(tài) X 的更新公式可得到該結(jié)論。

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

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

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

Matlab 實(shí)現(xiàn)functionkalmanFiltering%%clcclose all%%%%Deion:kalmanFiltering%Author:Liulongpo%Time：2015-4-2916:42:34%%%Z=(1:2:200);%觀測值汽車的位置也就是我們要修改的量noise=randn(1,100);%方差為1的高斯噪聲Z=Z+noise;X=[0;0];%初始狀態(tài)P=[10;01];%狀態(tài)協(xié)方差矩陣F=[11;01];%狀態(tài)轉(zhuǎn)移矩陣Q=[0.0001,0;0,0.0001];%狀態(tài)轉(zhuǎn)移協(xié)方差矩陣H=[1,0];%觀測矩陣R=1;%觀測噪聲方差figure;hold on;fori=1:100%基于上一狀態(tài)預(yù)測當(dāng)前狀態(tài)X_=F*X;%更新協(xié)方差Q系統(tǒng)過程的協(xié)方差這兩個公式是對系統(tǒng)的預(yù)測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)，橫軸表示位置，縱軸表示速度endend效果如下

其中 x 軸為位置，y軸為速度。

在代碼中，我們設(shè)定x的變化是 1:2:200，則速度就是2，可以由上圖看到，值經(jīng)過幾次迭代，速度就基本上在 2 附近擺動，擺動的原因是我們加入了噪聲。

接下來來看一個實(shí)際例子。

我們的數(shù)據(jù)為 data = [149.360 , 150.06, 151.44, 152.81,154.19 ,157.72];

這是運(yùn)用光流法從視頻中獲取角點(diǎn)的實(shí)際x軸坐標(biāo)，總共有6個數(shù)據(jù)，也就是代表了一個點(diǎn)的連續(xù)6幀的x軸坐標(biāo)。接下來這個例子，我們將實(shí)現(xiàn)用5幀的數(shù)據(jù)進(jìn)行訓(xùn)練，然后預(yù)測出第6幀的x軸坐標(biāo)。

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

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

代碼如下：

KF.m

function[predData,dataX]=KF(dataZ)%%%%Deion:kalmanFiltering%Author:Liulongpo%Time：2015-4-2916:42:34%%%Z=dataZ';len = length(Z);%Z=(1:2:200); %觀測值 汽車的位置 也就是我們要修改的量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]; %觀測矩陣R=1; %觀測噪聲方差%figure;%hold on;for i = 1:len%基于上一狀態(tài)預(yù)測當(dāng)前狀態(tài) % 2x1 2x1X_ = F*X;% 更新協(xié)方差 Q系統(tǒng)過程的協(xié)方差 這兩個公式是對系統(tǒng)的預(yù)測% 2x1 2x1 1x2 2x2P_ = 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)，橫軸表示位置，縱軸表示速度endpredData=F*X;end

testKF.m

functiontestKF%%clcclose 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);figuresubplot 311scatter(i,data,3),title('原始數(shù)據(jù)')subplot 312scatter(i,DataX(1,:),3),title('預(yù)測數(shù)據(jù)')subplot 313scatter(i,error,3),title('預(yù)測誤差')predData(1)%{scatter(i,error,3);figurescatter(i,data,3)figurescatter(i,predData(1,:),3)%}end效果如下：

預(yù)測結(jié)果為： 155.7493 ，跟實(shí)際結(jié)果 157.72 僅有1.9 的誤差，可以看到，卡爾曼濾波器算法對于少量數(shù)據(jù)的預(yù)測效果還是挺不錯的。當(dāng)然，預(yù)測位置的同時，我們也得到了預(yù)測速度

責(zé)任編輯：

總結(jié)

以上是生活随笔為你收集整理的卡尔曼滤波器求速度matlab,卡尔曼滤波器算法浅析及matlab实战的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。