Kalman濾波器的原理與實現
卡爾曼濾波器(Kalman Filter)是一個十分強大濾波器,雖然叫做濾波器，卡爾曼濾波器其實可以起到到兩個作用，即預測與更新，這與我們在其執行時所關注的環節有關。當我們關注預測狀態量這一步時，我們可以透過卡爾曼濾波器獲取狀態量的超前預測值，預測的值取決於濾波物件的運動建模。而當我們關注更新來獲取最優估計狀態時，我們關心的是如何透過卡爾曼增益以及測量量與估計量來獲取當前的最優估計狀態，這時卡爾曼濾波就像一個正常的濾波器。本篇是看完Dr_Can博士對於卡爾曼濾波的講解後的小筆記。
連結在這Dr_Can的卡爾曼濾波器講解
一些引數明確
在解釋卡爾曼濾波之前，需要先明確一些引數，如下
\(x_k\) 為\(k\)時刻的狀態量的真實值\(\\\)
\(\hat{x}_k^-\) 為\(k\)時刻的狀態量的先驗預測值\(\\\)
\(\hat{x}_{k-1}\) 為\(k-1\)時刻的狀態量的最優後驗估計值\(\\\)
\(z_k\) 為\(k\)時刻的測量量\(\\\)
\(u\) 為外部輸入的控制量\(\\\)
\(B\) 為控制量矩陣\(\\\)
\(A\) 為狀態轉移矩陣\(\\\)
\(P^-\) 為誤差協方差矩陣的先驗預測值\(\\\)
\(P\) 為誤差協方差矩陣\(\\\)
\(H\) 為觀測轉移矩陣\(\\\)
\(Q\) 為狀態噪聲協方差矩陣\(\\\)
\(R\) 為測量噪聲協方差矩陣\(\\\)
\(K_k\) 為卡爾曼增益\(\\\)
例子引出
給出一個勻速運動模型的例子來引出卡爾曼濾波。這也是一個經典的例子。
給出一個做勻速直線運動的小球，其速度為\(V\),其相對於某座標系的位置為\(x\),根據運動學相關的知識，可以寫出小球的運動方程\(x_{t} = x_{t-1} + V_x * t\)。假設小球不僅在\(x\)方向上有運動，在\(y\)方向上也有同樣的勻速直線運動，其\(y\)方向的運動方程為\(y_{t} = y_{t-1} + V_y * t\)。即我們現在有了描述小球運動的兩個方程,再加上速度的兩個方程\(V_{xt} = V_{xt-1}\)以及\(V_{yt} = V_{yt - 1}\)。我們稱物體的以上的變數為物體的狀態量，也就是我們想要獲得一些量。
不妨寫為矩陣相乘的形式
倘若假設取樣時間為1即\(t = 1\),可以寫成
可以看出，\(t\)時刻物體的狀態量可以由\(t-1\)時刻物體的狀態量來推算出來。此時我們將上式記為
顯然有時物體並不會一直線性運動，假設此時物體被外部因素賦予(輸入)一個加速度\(a\),則物體此時的運動方程變為
則矩陣形式的方程變為
不妨記為
這樣我們就得出瞭如何使用前一時刻或者說前一次的狀態量來獲得預測當前的狀態量。不難看出，預測的準不準，取決於對觀測物體運動建模的準確性。
倘若模型不準確，根據該模型推理出的資料是有噪聲干擾和誤差的,我們將這個噪聲干擾叫做\(w_i\)。並假設噪聲的機率分佈符合期望為0的正態分佈(這是卡爾曼濾波的重要假設之一)，即\(P(w_i)\sim N(0,Q)\)。加上這個誤差後的公式為
需要注意的是\(w_i\)也為一個矩陣，同理\(Q\)也為一個協方差矩陣,這是一個多元正態分佈。
此時我們再來考慮更多一種情況。倘若我們不僅可以透過模型對狀態量進行預測得出當前時刻的狀態量，同時還可以透過一些測量儀器來獲得物體的當前狀態的測量值。
在這個例子中可以認為有一個雷達來檢測物體每一時刻的位置資訊以及速度資訊。這些測量值統稱為\(z_k\),為一個矩陣。注意測量量的維度並不要求與狀態量相同。考慮到有時測量量與狀態量可能需要些許轉換，如測量電阻時我們總是使用電壓處以電流(\(R = \frac{U}{I}\))的轉換，我們將這種轉換關係使用一個矩陣\(H\)表示，\(H\)就表示狀態量可以透過某個公式或者關係轉換到測量量。在上面的例子中，\(H\)為單位矩陣。綜上，可以得出以下式子。
當然，考慮到測量儀器測量時的各種噪聲以及誤差，我們將其記為\(v_i\),並假設其機率分佈也滿足正態分佈，即\(P(v_i)\sim N(0,R)\)。
最終得出
最後，我們好像得出了觀測物體的兩個值，一個是透過不太精確的模型得出的預測值，另一個是透過不太準確的儀器測出的當前的測量值。我們所有的資訊只有這兩個值。那麼如何在一些合理的假設下，透過這兩個不太準確的值，獲得最接近物體真實值的最優解呢？這便是這便是卡爾曼濾波所要做的事情，透過兩個不準確值來估計出最接近真實值的最優解。
寫在題外：這兩個公式實質上的嚴格數學推導是由現代控制理論裡的狀態空間表示式的離散形式得出的，想要了解更多的推導請看Dr_Can的卡爾曼濾波器講解，講的非常詳細易懂！
卡爾曼濾波簡單推導
下面對卡爾曼濾波的一些思想和公式進行簡單的說明
資料融合
考慮最簡單的均值濾波，對於物體的某一屬性(如上述的位置\(x\))我們已經有\(k-1\)個已經測量到的值，以及當前第\(k\)次的測量值。為了利用當前這些資料獲取接近真實值的值，我們最容易想到的就是均值濾波，也就是從1到\(k\)的測量取均值。
不妨將其做以下變換
最終我們得出了這個式子
不難看出，當前\(k\)時刻的估計值由\(x_{k-1}\)時刻的估計值\(\hat{x_{k-1}}\)以及\(k\)時刻的測量值\(x_k\)來共同決定，而\(\frac{1}{k}\)這個係數決定了這兩者的佔比。我們不妨令\(\frac{1}{k} = G\)，這裡姑且稱其為卡爾曼增益。且\(\hat{x_{k-1}}\)可以視為基於前\(k-1\)次的資料對第\(k\)次的資料的一個預測或者說估計。不妨即為\(\hat{x_k}^-\)
利用這個思想，對於小球的例子中，\(x_{k}\)時刻的預測值可以由運動方程給出
且\(k\)時刻的測量量滿足以下
利用上述思想，得出\(k\)時刻的估計值為
不妨假設\(G = K_kH\),帶入得出
這便是卡爾曼濾波的核心公式之一，我們使用這個公式來估計當前狀態量的最優值。至於為何為最優，後續會給出簡單說明。
協方差矩陣
這部分是機率統計的知識。對於隨機變數\(X\)，我們可以用期望\(E(x)\)來描述它的平均水平或者一個趨勢，同時可以使用方差\(D(x)\)或者\(Var(x)\)來描述其波動情況。同時我們定義這樣一個式子
稱\(Cov(X,Y)\)為兩個隨機變數的協方差。不難看出，協方差描述了兩個變數之間的相關程度，具體可以看這個通俗理解協方差。當兩個變數相互獨立時，其協方差為0,說明彼此毫無關係，互不影響。
對於兩個隨機變數\(X\)和\(Y\)，可以給出一個矩陣來描述其各個變數之間的關係
這樣使用協方差矩陣就可以記錄出2個以及多個隨機變數之間的關係。
對於\(e = [e_1\quad e_2]\)假設隨機變數\(e_1,e_2\)符合正態分佈\((0,\sigma)\)(基本上卡爾曼濾波就是建立在所有誤差都是正態分佈這個假設上的),其各個元素協方差矩陣可以這樣求
展開即為
已知方差的計算式有\(D(x) = E(x^2) - E^2(x)\),且我們已知符合期望為0的正態分佈，則\(E(x) = 0\)，故有\(D(x) = E(x^2)\)，同理對於協方差的計算式\(Cov(X,Y) = E(XY) - E(X)E(Y)\),應用如上條件變為\(Cov(X,Y) = E(XY)\),這兩個公式應用於上式有
這個公式在推導卡爾曼增益的計算式時會用到。
卡爾曼公式簡單推導
先給出卡爾曼濾波的五個核心公式，然後對其做一些說明
[1]式
對於1式，我們將上一部分推斷的公式寫到這裡
估計時，誤差已經融入估計的計算，也就是誤差與噪聲是包含在模型本身中的，無法像上面一樣直接提出來一個\(w_i\),因此我們將這個帶有誤差與噪聲的預測值記為\(\hat{x_k}^-\),代表著狀態量的先驗預測值。這樣我們就得出來第一個公式
[4]式
對於4式，我們可以看到出現了當前的最優估計值\(\hat{x_{k}}\)以及先驗預測值\(\hat{x_{k}}^-\)和測量量\(z_k\)。還有一個新的量\(K_k\),我們稱之為卡爾曼增益。卡爾曼增益是卡爾曼濾波中最為靈魂最為重要的一個量。
不難看出，當\(K_k = 0\)時(實際上是一個全0矩陣),\(x\)在\(k\)時刻的最優估計值\(\hat{x_k}\)就為\(k\)時刻的先驗預測值\(\hat{x_{k}}^-\),當\(K_k = H\)時，\(\hat{x_k}\)就為測量值\(H^-z_k\)(這是由測量量\(z_k = Hx_k\)兩邊同乘\(H^-\)得\(x_k = H^-z _k\))。因此\(K_k\)這個引數決定了測量量與預測量對於最優估計值的貢獻程度。
利用上面中資料融合的思想可以知道，為了求出當前狀態下最優的估計值，我們唯一不知道的量就是\(K_k\)。而衡量最優的條件是什麼呢？顯然在機率統計中，方差越小，資料圍繞真實值越集中。因此我們此時的目標就是尋找一個\(K_k\)的計算式，這樣的\(K_k\)使得\( x_k -\hat{x_k}\)的方差(多後設資料使用協方差矩陣的跡)最小。
[3]式
再講這個式子之前，我們不妨給出一個真實狀態量與後驗最優估計量之間的誤差的量化。
同理，假設\(e_k\)的誤差也符合正態分佈\(P(e_k)\sim{N(0,P)}\)。與噪聲的方差一樣，\(P\)也是一個協方差矩陣，反映了\(e_k\)的各個狀態之間的關係以及誤差。以勻速運動的物體為例,我們僅關注其位置為狀態量。
因此當前的目的就是尋求一個\(K_k\)，使得\(e_k\)的誤差最小，也就是協方差矩陣的跡最小(跡上的元素描述的是各個變數相對於其真實值的誤差，而其他元素描述的是各個變數之間的相關性)。我們依舊認為\(e_k\)符合正態分佈\((0,P)\),由上述協方差矩陣的計算式可知
帶入\(e_k\)
又已知\(\hat{x_k}\)的表示式為
將其帶入計算P的式子，這樣\(K_k\)就引入進來了。接著只需要應用一些線性代數的技巧化簡即可。最後對得到的式子求導取極值可以得到\(K_k\)的計算式
其中
[2]式
對於[2]式，是由\(k-1\)次的誤差協方差矩陣推算出\(k\)次的先驗誤差協方差矩陣的過程。使用了計算公式\(D(AX) = AD(X)A^T\),外加上噪聲矩陣Q的測量噪聲，可以得出[2]式
這是很不嚴謹的推理，算不上推導，推導請看Dr_Can的卡爾曼濾波講解
[5]式
最後，我們要更新誤差協方差矩陣，得出根據\(\hat{x_k}\)計算出的最新的誤差協方差矩陣。直接使用公式
帶入求出表示式即可。最終得出
綜上，[1][2]式用來根據模型進行當前狀態的預測，[3][4][5]式用來對當前狀態進行最優估計，這兩部分不斷迴圈執行，一個卡爾曼濾波器就開始工作了。
相對於簡單的均值濾波，我們可以看到卡爾曼濾波並不太依賴於之前的一些資料與狀態，本次的更新僅取決於上一次的資料，而且可以取得不錯的效果。而均值的話非常依賴之前的樣本數量，樣本數量少的話會導致本次的估計不太準確
至此，卡爾曼濾波的黃金五式就解釋完啦，知識水平有限，講的很粗糙，有錯誤的話記得提醒我。
簡單擴充套件Kalman濾波器
顯然，kalman濾波器的預測的第一個式子
註定了卡爾曼濾波只能使用與線性系統，且系統的各種誤差與噪聲均符合期望為0的正態分佈。那麼，對於非線性系統，怎麼才可以使用卡爾曼濾波呢？答案是線性化。
對於一個使用非線性模型描述的系統(即包含\(sinx，cosx，e^x\))，我們可以透過泰勒展開的方式將其線性化。透過非線性函式在上一次最優估計\(\hat{x_{k-1}}\)處的一階泰勒展開，將其化為多項式的形式。對於一些矩陣如A,B,H等，可以利用矩陣求導的知識將其化為對應的雅可比矩陣來求解。
也就是說擴充套件卡爾曼濾波與卡爾曼濾波的不同之處僅在於預測這一步中的模型是非線性系統線性化後的模型，其他的步驟與普通而卡爾曼濾波一模一樣。
c++實現
下面給出一個用Eigen庫以及模板寫的卡爾曼濾波，其中模板引數1為狀態量的維度，2為測量量的維度
kalman.h
#ifndef _KALMAN_H_
#define _KALMAN_H_
#include <Eigen/Core>
#include <Eigen/Dense>
/*
* Kalman濾波模板類，不考慮控制量B以及u
* 模板dimx = 狀態量維度，dimz = 測量量維度
*/
template <int dimx, int dimz>
class Kalman
{
/*
* 常用矩陣定義
*/
public:
using Vec_x = Eigen::Matrix<double, dimx, 1>;
using Vec_z = Eigen::Matrix<double, dimz, 1>;
using Mat_xx = Eigen::Matrix<double, dimx, dimx>;
using Mat_zx = Eigen::Matrix<double, dimz, dimx>;
using Mat_xz = Eigen::Matrix<double, dimx, dimz>;
using Mat_zz = Eigen::Matrix<double, dimz, dimz>;
private:
Vec_x x; // 狀態量，dimx * 1 維度列向量
Vec_z z; // 測量量，dimz * 1 維度列向量
Mat_xx A; // 狀態轉移矩陣， dimx * dimx 維度矩陣
Mat_zx H; // 觀測轉移矩陣， dimz * dimx 維度矩陣
Mat_xx P; // 狀態量的誤差協方差矩陣， dimx * dimx 維度矩陣
Mat_xx Q; // 狀態噪聲協方差矩陣， dimx * dimx 維度矩陣
Mat_zz R; // 測量噪聲協方差矩陣， dimz * dimz 維度矩陣
Mat_xz K; // 卡爾曼增益， dimx * dimz 維度矩陣
Mat_xx I; // 單位矩陣， dimx * dimx 維度矩陣
public:
Kalman();
~Kalman();
void Init(Vec_x _x, Mat_xx _P, Mat_xx A_, Mat_zx H_, Mat_xx Q_, Mat_zz R_);
Vec_x Predict();
Vec_x Predict(Mat_xx B, Vec_x u);
void Update(Vec_z _z);
Vec_x Get_Best_x();
};
template class Kalman<2, 2>;
template class Kalman<4, 2>;
template class Kalman<4, 4>;
#endif
kalman.cpp
#include "Kalman.h"
#include <unistd.h>
#include <iostream>
using namespace std;
template <int dimx, int dimz>
Kalman<dimx, dimz>::Kalman()
{
}
template <int dimx, int dimz>
Kalman<dimx, dimz>::~Kalman()
{
}
template <int dimx, int dimz>
void Kalman<dimx, dimz>::Init(Vec_x _x, Mat_xx _P, Mat_xx _A, Mat_zx _H, Mat_xx _Q, Mat_zz _R)
{
this->x = _x;
this->P = _P;
this->A = _A;
this->H = _H;
this->Q = _Q;
this->R = _R;
this->I.setIdentity();
}
/*
* 這裡的Kalman<dimx, dimz>::Vec_x是整一個返回型別，因為Vec_x是在類內定義的型別
*/
template <int dimx, int dimz>
typename Kalman<dimx, dimz>::Vec_x Kalman<dimx, dimz>::Predict()
{
x = A * x; // 狀態預測估計，不帶輸入
P = A * P * A.transpose() + Q; // 預測估計誤差協方差矩陣
return x;
}
template <int dimx, int dimz>
typename Kalman<dimx, dimz>::Vec_x Kalman<dimx, dimz>::Predict(Mat_xx B, Vec_x u)
{
x = A * x + B * u; // 狀態預測估計，帶輸入
P = A * P * A.transpose() + Q; // 預測估計誤差協方差矩陣
return x;
}
template <int dimx, int dimz>
void Kalman<dimx, dimz>::Update(Vec_z _z)
{
K = P * H.transpose() * (H * P * H.transpose() + R).inverse(); // 計算卡爾曼增益
x = x + K * (_z - H * x); // 更新最優狀態值
P = (I - K * H) * P;
}
template <int dimx, int dimz>
typename Kalman<dimx, dimz>::Vec_x Kalman<dimx, dimz>::Get_Best_x()
{
return x;
}