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

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

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

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

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

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

如下圖：

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

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

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

協方差矩陣

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

可以看到，服從高斯分佈的一維資料大部分分佈在均值附近。

現在我們來看看服從高斯分佈的二維資料投影到座標軸的情況，如下圖：

二維資料比一維資料稍微複雜一點，投影后有3種情況，分別是：
左圖：兩個維的資料互不相關；
中圖：兩個維的資料正相關，也就是 y 隨著 x 的增大而增大（假設兩個維分別為 x 和 y）
右圖：兩個維的資料負相關，也就是 y 隨著 x 的增大而減小。

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

狀態協方差矩陣傳遞

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

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

也即：

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

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

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

在這裡，我們的實際資料就是 Z， 如下圖：

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

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

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

K的作用：

1.K 權衡預測協方差P和觀察協方差矩陣R那個更加重要，相信預測，殘差的權重小，相信觀察，殘差權重大，由 K 的表達是可以退出這個結論
2,將殘差的表現形式從觀察域轉換到狀態域（殘差與一個標量，通過K 轉換為向量），由 狀態 X 的更新公式可得到該結論。

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

狀態協方差的更新

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

Matlab 實現

function kalmanFiltering
%%
clc
close all

%%
%
%   Description : kalmanFiltering
%   Author : Liulongpo
%   Time：2015-4-29 16:42:34
%

%%
Z=(1:2:200); %觀測值  汽車的位置  也就是我們要修改的量
noise=randn(1,100); %方差為1的高斯噪聲
Z=Z+noise;
X=[0 ; 0 ]; %初始狀態
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: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); %畫點，橫軸表示位置，縱軸表示速度
end

end

Matlab效果

其中 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)

%%
%
%   Description : kalmanFiltering
%   Author : Liulongpo
%   Time：2015-4-29 16: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  2x1
X_ = F*X;
% 更新協方差  Q系統過程的協方差  這兩個公式是對系統的預測
%   2x1  2x1  1x2  2x2
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_;
dataX(:,i) = [X(1);X(2)];
%scatter(X(1), X(2),4); %畫點，橫軸表示位置，縱軸表示速度
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('原始資料')
subplot 312
scatter(i,DataX(1,:),3),title('預測資料')
subplot 313
scatter(i,error,3),title('預測誤差')
predData(1)
%{
scatter(i,error,3);
figure
scatter(i,data,3)
figure
scatter(i,predData(1,:),3)
%}
end

測試效果：

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