前言

LZ最近在看Udacity的無人駕駛課程，該課程主要分爲三部分，第一部分的課程主要使用Python實現的車道線識別、車牌識別等計算機視覺項目。由於我對定位、建圖等方面有些知識儲備，所以先從第二部分課程開始。
本節將用最簡潔的話講解卡爾曼濾波KF、非線性卡爾曼濾波EKF等知識點，並就此實現一個多傳感器融合定位的小demo，後面會就粒子濾波PF專門開一個章節講解。

對於勻速運動模型，KF和EKF的狀態預測（Predict）過程是一樣的；KF和EKF唯一區別的地方在於測量值更新（Update）這一步。在測量值更新中，KF使用的測量矩陣H是不變的，而EKF的測量矩陣H是Jacobian矩陣。

本文約束：勻速運動；固定位置的傳感器對小車不斷測量。

一、卡爾曼濾波 KF

1、引子

1）兩個傳感器測量同一個信號，爲了減小誤差我們可以採用取平均的方式，
2）進一步的我們採用加權平均（由方差大小分配），但加權平均是一種靜態分配方式。3）方差是隨外界環境而變的，加權值也應該隨之改變，這就是卡爾曼濾波出現的原因，它是一種動態更新加權值，不斷迭代的算法。

卡爾曼濾波器就是根據上一時刻x(k-1)的狀態，預測當前時刻x(k)的狀態，將預測的狀態x^(k)與當前時刻的測量值z進行加權，加權後的結果才認爲是當前的實際狀態。

2、對象：線性高斯模型，這個模型最好的地方在於兩個高斯分佈的乘積仍然是一高斯分佈，由此實現模型的動態迭代。

3、本質：參數化的貝葉斯模型。先驗估計：對下一時刻系統初步狀態估計；結合先驗估計以及測量反饋，得到後驗估計。

4、應用：最優化自迴歸數據處理算法、機器人導航、雷達系統、圖像處理等

2、黃金公式及推導

直接上結論，下面這個公式分爲兩部分，上面的是預測，下面是更新。

具體公式推導，我竟然發現了我之前的筆記，下面直接上圖解釋。
爲了避免推導晦澀難懂，採用小汽車的模型進行一步一步的推導。

1、建立模型

2、簡單一維小車

假設有個小車勻速運動，我們在左側安裝了一個測量小車距離和速度傳感器，每秒測一次小車位置s和速度v。我們用向量 $x_{t}$ 狀態向量。

由於測量誤差的存在，無法獲得真實值，我們可以以一個高斯分佈來表示結果在各個地方出現的概率，如下圖所示，在真值附近的概率最高。

$x_{t-1}=\begin{bmatrix} s_{t-1}\\ v_{t-1} \end{bmatrix}$
接下來是使用歷史信息對未來的位置進行推測，以速度v勻速行走 $\Delta t$ 後，小車位置的紅色區域範圍變大了，這是因爲預測時加入了速度估計的噪聲，放大了不確定性。

$\begin{bmatrix}s_{t}\\v_{t}\end{bmatrix}= \begin{bmatrix}1 & \Delta t\\ 0& 1\end{bmatrix}\begin{bmatrix}s_{t-1}\\v_{t-1}\end{bmatrix}$
此時傳感器對小車做了一次觀測,結果爲 z

上圖藍色區域爲此時觀測結果，紅色爲預測結果，那麼最終的結果是怎樣的呢？下圖綠色部分給出了答案。

我們對預測和觀測結果用不同的權重得到最終結果，兩個權值的計算是根據預測結果和觀測結果的不確定性來的，這個不確定性就是高斯分佈中的方差的大小，方差越大，波形分佈越廣，不確定性越高，這樣一來給的權值就會越低。

3、預測（Predict）

上面是一維小車，狀態向量裏只有它的位置和速度，當二維小車在平面時它的狀態向量有分別是位置和速度的x、y方向（x,y,vx,vy）
$x_{}=\begin{bmatrix}x\\ y\\vx\\vy\end{bmatrix}$

和上面的一維對應
$\begin{bmatrix}s_{t}\\v_{t}\end{bmatrix}= \begin{bmatrix}1 & \Delta t\\ 0& 1\end{bmatrix}\begin{bmatrix}s_{t-1}\\v_{t-1}\end{bmatrix}$
這裏的二維應該是

$\begin{bmatrix}x_{t}\\ y_{t}\\vx_{t}\\vy_{t}\end{bmatrix}=\begin{bmatrix} 1& 0 & \Delta t& 0\\ 0 & 1 & 0 & \Delta t\\ 0 & 0& 1 & 0\\ 0 & 0& 0 & 1 \end{bmatrix}\begin{bmatrix}x_{t-1}\\ y_{t-1}\\vx_{t-1}\\vy_{t-1}\end{bmatrix}$ +Bu
接下來是預測的第二個模塊，狀態協方差矩陣P和過程噪聲Q。P表示系統的不確定程度，卡爾曼濾波器初始化時很大，隨着更多數據注入濾波器，不確定度會逐漸變小。Q暫時設爲單位矩陣。

$P=\begin{bmatrix} 1& 0 & 0& 0\\ 0 & 1 & 0 & 0\\ 0 & 0& 100 & 0\\ 0 & 0& 0 & 100 \end{bmatrix}$
$Q=\begin{bmatrix} 1& 0 & 0& 0\\ 0 & 1 & 0 & 0\\ 0 & 0& 1 & 0\\ 0 & 0& 0 & 1\end{bmatrix}$

4、觀測更新（measurement update）

先說第一個公式括號裏面的z-Hx，z表示的是測量值，x是4維的狀態向量，對於激光雷達測量的z就是位置（x，y），對於毫米波雷達測量的z是位置和角度。需要對狀態向量x左乘一個矩陣H，才能將二者映射到同一個空間，直接進行加減運算。
以激光雷達的測量值z爲：
$z=\begin{bmatrix}x_{m}\\y_{m}\end{bmatrix}$
z-Hx擴展開爲：
$\begin{bmatrix}\Delta x\\\Delta y\end{bmatrix}=\begin{bmatrix}x_{m}\\y_{m}\end{bmatrix}-\begin{bmatrix} 1& 0& 0 & 0\\ 0& 1 & 0 & 0 \end{bmatrix}*\begin{bmatrix}x_{t}\\ y_{t}\\vx_{t}\\vy_{t}\end{bmatrix}$
測量矩陣 H 是一個2*4的矩陣
$H=\begin{bmatrix} 1& 0& 0 & 0\\ 0& 1 & 0 & 0 \end{bmatrix}$

    H_lidar_ = Eigen::MatrixXd(2, 4);
    H_lidar_ << 1, 0, 0, 0,
                0, 1, 0, 0;

接下來說K(z-Hx)中的權重K是如何取的，簡單來說是和協方差矩陣相關。

    R_lidar_ = Eigen::MatrixXd(2, 2);
    R_lidar_ << 0.0225, 0,
                0, 0.0225;

這兩個公式求的是卡爾曼濾波器中一個很重要的量——卡爾曼增益K（Kalman Gain），用人話講就是求y值的權值。第一個公式中的R是測量噪聲矩陣（measurement covariance matrix），這個表示的是測量值與真值之間的差值,R應該是2*2的矩陣。一般情況下，傳感器的廠家會提供該值。

求得K之後，當前時刻的x和P都可以求解出來了，第一個公式是完成了當前狀態向量x的更新，不僅考慮了上一時刻的預測值，也考慮了測量值，和整個系統的噪聲，第二個公式根據卡爾曼增益，更新了系統的不確定度P，用於下一個週期的運算。

二、非線性卡爾曼濾波EKF

KF中測量矩陣H是固定的，代碼中直接賦值即可。
$H=\begin{bmatrix} 1& 0& 0 & 0\\ 0& 1 & 0 & 0 \end{bmatrix}$
但是真實情況下，測量矩陣 H應該是需要求雅克比矩陣求出的。我們接下來以毫米波雷達爲例

毫米波原理是多普勒效應，能夠測量障礙物在極座標系下雷達的距離ρ，方向角ϕ和距離的變化率（徑向速度）ρ’。

已知量：狀態向量x爲4*1，

已知量：觀測值z的數據維度爲3*1.

觀測值z和預測值x之間的差值y關係爲：

但是如果轉化爲 y=Hx時，由於第二部分的轉化是非線性的，無法找到一個常數矩陣H。

所以我們需要將上述非線性函數轉化爲近似的線性函數，用一階泰勒展開。對狀態向量x求偏導數，即雅克比Jacobian矩陣。

三、代碼講解

KF和EKF都在kalmanfilter.h/cpp文件中，頭文件KalmanFilter類爲：

class KalmanFilter {
public:
    // 構造函數和析構函數
    KalmanFilter();
    ~KalmanFilter();
    // 初始化
    void Initialization(Eigen::VectorXd x_in);
    bool IsInitialized();

    // 5個矩陣賦值
    void SetF(Eigen::MatrixXd F_in);
    void SetP(Eigen::MatrixXd P_in);
    void SetQ(Eigen::MatrixXd Q_in);
    void SetH(Eigen::MatrixXd H_in);
    void SetR(Eigen::MatrixXd R_in);
    // 預測
    void Prediction();
    // KF更新
    void KFUpdate(Eigen::VectorXd z);
    // EKF更新
    void EKFUpdate(Eigen::VectorXd z);
    // 獲得狀態x
    Eigen::VectorXd GetX();

private:
    // Jacobian矩陣
    void CalculateJacobianMatrix();

    // 是否初始化標誌
    bool is_initialized_;

    // 狀態向量x
    Eigen::VectorXd x_;

    // 狀態協方差矩陣P
    Eigen::MatrixXd P_;
    // 狀態轉移矩陣F  
    Eigen::MatrixXd F_;
    // 過程噪聲Q 
    Eigen::MatrixXd Q_;
    // 測量矩陣H 
    Eigen::MatrixXd H_;
    // 測量噪聲R 
    Eigen::MatrixXd R_;
};

主要成員變量爲：狀態轉移矩陣F、狀態協方差矩陣P、測量矩陣H、過程噪聲Q、測量噪聲R、狀態向量x
主要成員函數爲：初始化Initialization()、5個矩陣賦值set()、預測prediction()、KF更新KFUpdate()、EKF更新EKFUpdate().

1、預測Prediction()

// x=F*x
// P=F*P*Ft+Q
void KalmanFilter::Prediction()
{
    x_ = F_ * x_;
    Eigen::MatrixXd Ft = F_.transpose();
    P_ = F_ * P_ * Ft + Q_;
}

2、KF更新KFUpdate()

// x=x+k*(z-H*x)
// P=(I-K*h)*P
// K=P*Ht*(H*P*Ht+R)t
void KalmanFilter::KFUpdate(Eigen::VectorXd z)
{
    Eigen::VectorXd y = z - H_ * x_;
    Eigen::MatrixXd Ht = H_.transpose();
    Eigen::MatrixXd S = H_ * P_ * Ht + R_;
    Eigen::MatrixXd Si = S.inverse();
    Eigen::MatrixXd K =  P_ * Ht * Si;
    x_ = x_ + (K * y);
    int x_size = x_.size();
    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(x_size, x_size);
    P_ = (I - K * H_) * P_;
}

3、EKF更新EKFUpdate()

EKF的預測兩個公式和之前一樣，都是Prediction()函數，區別在於求解測量矩陣H矩陣。
首先是要求解測量值z和預測值x之間的偏差。y=z-Hx。
這裏Hx用的就是將x從笛卡爾座標系轉化爲極座標系。
接下來的主要問題是在求解H矩陣。

// KF和EKF區別在於測量矩陣H的計算
void KalmanFilter::EKFUpdate(Eigen::VectorXd z)
{
    double rho = sqrt(x_(0)*x_(0) + x_(1)*x_(1));
    double theta = atan2(x_(1), x_(0));
    double rho_dot = (x_(0)*x_(2) + x_(1)*x_(3)) / rho;
    Eigen::VectorXd h = Eigen::VectorXd(3);
    h << rho, theta, rho_dot;
    Eigen::VectorXd y = z - h;

    CalculateJacobianMatrix();

    Eigen::MatrixXd Ht = H_.transpose();
    Eigen::MatrixXd S = H_ * P_ * Ht + R_;
    Eigen::MatrixXd Si = S.inverse();
    Eigen::MatrixXd K =  P_ * Ht * Si;
    x_ = x_ + (K * y);
    int x_size = x_.size();
    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(x_size, x_size);
    P_ = (I - K * H_) * P_;
}

4、CalculateJacobianMatrix()函數

void KalmanFilter::CalculateJacobianMatrix()
{
    Eigen::MatrixXd Hj(3, 4);

    // get state parameters
    float px = x_(0);
    float py = x_(1);
    float vx = x_(2);
    float vy = x_(3);

    // pre-compute a set of terms to avoid repeated calculation
    float c1 = px * px + py * py;
    float c2 = sqrt(c1);
    float c3 = (c1 * c2);

    // Check division by zero
    if(fabs(c1) < 0.0001){
        H_ = Hj;
        return;
    }

    Hj << (px/c2), (py/c2), 0, 0,
         -(py/c1), (px/c1), 0, 0,
          py*(vx*py - vy*px)/c3, px*(px*vy - py*vx)/c3, px/c2, py/c2;
    H_ = Hj;
}

5、main主函數

主函數基本流程爲：

int main()
{
    KalmanFilter kf;
    while(getlint(x,y,時間戳)){
         if(激光雷達尚未初始化){
             kf.Initialization(x_in);
             P<< 4*4;
             Q<< 4*4;
             H<< 2*4;
             R<<2*2;
         }
         獲取兩幀時間差delta t;
         F<< 4*4;
         kf.Prediction();
         kf.KFUpdate();
         VectorXd x_out=kf.GetX();
    }
}

參考：
https://zhuanlan.zhihu.com/p/45238681

多傳感器融合定位1（激光雷達+毫米波雷達）

前言

一、卡爾曼濾波 KF

1、引子

2、黃金公式及推導

1、建立模型

2、簡單一維小車

3、預測（Predict）

4、觀測更新（measurement update）

二、非線性卡爾曼濾波EKF

三、代碼講解

1、預測Prediction()

2、KF更新KFUpdate()

3、EKF更新EKFUpdate()

4、CalculateJacobianMatrix()函數

5、main主函數

VINS-Mono 代碼詳細解讀——初始化1：視覺SFM詳解 processImage（）+initialStructure（）

多傳感器融合定位2（封裝KF和EKF）

Endnote連接Word自動插入畢業論文參考文獻

《wiki官網教程》2 編寫簡單的服務器service和客戶端 client(C++)

VINS-Mono 代碼詳細解讀——基於滑動窗口的緊耦合後端非線性優化 optimization()+滑動窗slideWindow()

Mac下配置sublime實現LaTeX

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結