原標題：卡爾曼濾波器算法淺析及matlab實戰

作者：Liu_LongPo

出處：Liu_LongPo的博客

卡爾曼濾波器是一種利用線性系統狀態方程，通過系統輸入輸出觀測數據，對系統狀態進行最優估計的算法。而且由于觀測包含系統的噪聲和干擾的影響，所以最優估計也可看做是濾波過程。

卡爾曼濾波器的核心內容就是5條公式，計算簡單快速，適合用于少量數據的預測和估計。

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

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

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

用向量表示上述轉換過程，如下：

如下圖：

那么我們可以得到如下的狀態轉移公式：

(1)

其中矩陣 F 為狀態轉移矩陣，表示如何從上一狀態來推測當前時刻的狀態，B 為控制矩陣，表示控制量u如何作用于當前矩陣，上面的公式 x 有頂帽子，表示只是估計值，并不是最優的。

有了狀態轉移公式就可以用來推測當前的狀態，但是所有的推測都是包含噪聲的，噪聲越大，不確定越大，協方差矩陣用來表示這次推測帶來的不確定性

協方差矩陣

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

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

現在我們來看看服從高斯分布的二維數據投影到坐標軸的情況，如下圖：

二維數據比一維數據稍微復雜一點，投影后有3種情況，分別是：

左圖：兩個維的數據互不相關；

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

右圖：兩個維的數據負相關，也就是 y 隨著 x 的增大而減小。

那怎么來表示兩個維的數據的相關性呢？答案就是協方差矩陣。

狀態協方差矩陣傳遞

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

那么加入狀態轉換矩陣 F ，得到

(2)

也即：

因此我們便得到了協方差的轉換公式。

現在我們得到了兩個公式，運用這兩個公式能夠對現在狀態進行預測。按照我們的正常思路來理解，預測結果不一定會對嘛，肯定有誤差。而且在我們大多數回歸算法或者是擬合算法中，一般思路都是先預測，然后看看這個預測結果跟實際結果的誤差有多大，再根據這個誤差來調整預測函數的參數，不斷迭代調整參數直到預測誤差小于一定的閾值。

卡爾曼算法的迭代思想也類似，不過這里根據誤差調整的是狀態 X 。

在這里，我們的實際數據就是 Z， 如下圖：

其中，矩陣 H 為測量系統的參數，即觀察矩陣，v 為觀測噪聲， 其協方差矩陣為R

那么我們的狀態更新公式如下：

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

K的作用：

1.K 權衡預測協方差P和觀察協方差矩陣R那個更加重要，相信預測，殘差的權重小，相信觀察，殘差權重大，由 K 的表達是可以退出這個結論

2,將殘差的表現形式從觀察域轉換到狀態域(殘差與一個標量，通過K 轉換為向量)，由 狀態 X 的更新公式可得到該結論。

至此，我們已經得到了 t 狀態下的最優估計值 Xt。但為了能讓我們的迭代算法持續下去，我們還必須更新狀態協方差的值。

狀態協方差的更新

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

Matlab 實現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];%初始狀態P=[10;01];%狀態協方差矩陣F=[11;01];%狀態轉移矩陣Q=[0.0001,0;0,0.0001];%狀態轉移協方差矩陣H=[1,0];%觀測矩陣R=1;%觀測噪聲方差figure;hold on;fori=1:100%基于上一狀態預測當前狀態X_=F*X;%更新協方差Q系統過程的協方差這兩個公式是對系統的預測P_=F*P*F'+Q;% 計算卡爾曼增益K = P_*H'/(H*P_*H'+R);% 得到當前狀態的最優化估算值 增益乘以殘差X = X_+K*(Z(i)-H*X_);%更新K狀態的協方差P = (eye(2)-K*H)*P_;scatter(X(1), X(2),4); %畫點，橫軸表示位置，縱軸表示速度endend效果如下

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

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

接下來來看一個實際例子。

我們的數據為 data = [149.360 , 150.06, 151.44, 152.81,154.19 ,157.72];

這是運用光流法從視頻中獲取角點的實際x軸坐標，總共有6個數據，也就是代表了一個點的連續6幀的x軸坐標。接下來這個例子，我們將實現用5幀的數據進行訓練，然后預測出第6幀的x軸坐標。

在上一個matlab例子中，我們的訓練數據比較多，因此我們的初始狀態設置為[0,0]，也就是位置為0，速度為0，在訓練數據比較多的情況下，初始化數據為0并沒有關系，因為我們在上面的效果圖中可以看到，算法的經過短暫的迭代就能夠發揮作用。

但在這里，我們的訓練數據只有5幀，所以為了加快訓練，我們將位置狀態初始化為第一幀的位置，速度初始化為第二幀與第一幀之差。

代碼如下：

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) ]; %初始狀態 分別為 位置 和速度P=[1 0;0 1]; %狀態協方差矩陣F=[1 1;0 1]; %狀態轉移矩陣Q=[0.0001,0;0 , 0.0001]; %狀態轉移協方差矩陣H=[1,0]; %觀測矩陣R=1; %觀測噪聲方差%figure;%hold on;for i = 1:len%基于上一狀態預測當前狀態 % 2x1 2x1X_ = F*X;% 更新協方差 Q系統過程的協方差 這兩個公式是對系統的預測% 2x1 2x1 1x2 2x2P_ = F*P*F'+Q;%計算卡爾曼增益K=P_*H'/(H*P_*H'+R);%得到當前狀態的最優化估算值增益乘以殘差X=X_+K*(Z(i)-H*X_);%更新K狀態的協方差P=(eye(2)-K*H)*P_;dataX(:,i)=[X(1);X(2)];%scatter(X(1),X(2),4);%畫點，橫軸表示位置，縱軸表示速度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('原始數據')subplot 312scatter(i,DataX(1,:),3),title('預測數據')subplot 313scatter(i,error,3),title('預測誤差')predData(1)%{scatter(i,error,3);figurescatter(i,data,3)figurescatter(i,predData(1,:),3)%}end效果如下：

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

責任編輯：

總結

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

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。