濾波算法（四）—— 卡爾曼濾波算法

一、算法介紹

卡爾曼濾波是一個神奇的濾波算法，應用非常廣泛，它是一種結合先驗經驗、測量更新的狀態估計算法。

1、狀態估計

首先，對於一個我們關心的物理量，我們假設它符合下面的規律

$x_{k}=ax_{k-1}$

其中， $x_{k}$ 爲該物理量本週期的實際值， $x_{k-1}$ 爲該物理量上一個週期的實際值，當然這個物理量可能不符合這個規律，我們只是做了一個假設。不同的物理量符合的規律不同，是我們的經驗，我們根據這個規律可以預測我們關心的物理量。比如，我們關心的物理量是車速，如果車輛接近勻速運動時，則的取值爲1，也就是這個週期與上個週期的速度相同。

下面我們再來看一下這個物理量的測量公式

$z_{k}=x_{k}+v_{k}$

其中， $z_{k}$ 是這個物理量的測量值， $v_{k}$ 是測量噪聲。我們對一個物理進行預測，測量是一個必不可少的手段，雖然測量的不一定準，但是在很大程度上體現了物理量的實際值。這個公式體現的就是實際值與測量值的關係。還是以車速爲例， $z_{k}$ 是通過車速傳感器得到的測量值。

實際中，物理量一般不會像我們上面的公式那樣簡單，一般我們用下面的公式來表示物理量的裝

$x_{k}=ax_{k-1}+bu_{k}$

其中， $bu_{k}$ 代表了處理噪聲，這個噪聲是處理模型與實際情況的差異，比如車速，他會受到人爲加速、減速、路面不平等外界因素的影響。

卡爾曼濾波的基本思想是綜合利用上一次的狀態和測量值來對物理量的狀態進行預測估計。我們用 $\hat{x_{k}}$ 來表示 $x_{k}$ 的估計值，則有下面的公式

$\hat{x}_{k}=\hat{x}_{k-1}+g_{k}\left ( z_{k}-\hat{x}_{k-1} \right )$

在這個公式中，綜合利用了上一個週期的估計值和這個週期的測量值來對 $x_{k}$ 進行估計。其中， $g_{k}$ 叫做卡爾曼增益，這個公式與一階濾波很相似，只不過卡爾曼增益是會變的，每個週期都會更新，一階濾波的係數則是固定值。考慮極端的情況來分析增益的作用，當 $g_{k}=0$ 時，增益爲0，這時 $\hat{x}_{k}=\hat{x}_{k-1}$ ，這表示我們這個週期的估計值與上個週期是相同的，不信任當前的測量值；當 $g_{k}=1$ 時，增益爲1，這時 $\hat{x}_{k}=z_{k}$ ，這表示我們這個週期的估計值與測量值是相同的，不信任上個週期的估計值，在實際應用時， $g_{k}$ 介於0~1之間，它代表了對測量值的信任程度。

2、卡爾曼增益

上面我們通過卡爾曼增益來估計物理量的值，那卡爾曼增益又是如何取值的呢？我們通過下面兩個公式來計算並在每個週期進行迭代更新。

$g_{k}=p_{k-1}/\left ( p_{k-1}+r \right )$

$p_{k}=\left ( 1-g_{k} \right )p_{k-1}$

在上述公式中，是測量噪聲 $v_{k}$ 的平均值，測量噪聲是符合高斯分佈的，一般可以從傳感器廠商那裏獲得測量噪聲的均值，如果無法獲得可以根據採集到的數據給出一個經驗值。的大小對最終濾波效果的影響是比較大的。 $p_{k}$ 爲本週期的預測誤差。我們採用分析卡爾曼增益的方法來分析預測誤差的作用，即採用假設極端情況的方法。假設前一次的預測誤差 $p_{k-1}=0$ ，根據第一個公式則 $g_{k}=0$ ，根據上面的分析，這種情況估計值爲上個週期的估計值；如果前一次的預測誤差 $p_{k-1}=1$ ，則增益變爲 $1/\left ( 1+r \right )$ ，一般取值很小，所以 $g_{k}\approx 1$ ，這種情況以新測量的值作爲估計值。

對於第二個公式，當卡爾曼增益爲0時， $p_{k}=p_{k-1}$ ，即採用上一個週期的預測誤差；當增益爲1時， $p_{k}=0$ 。

3、完整卡爾曼濾波算法

有了上面的推導，我們在下面列出來完成卡爾曼濾波的公式，卡爾曼濾波分爲預測過程和更新過程兩個過程，在公式中，我們又引入了縮放係數，和協方差。

預測過程：

$\hat{x}_{k}=a\hat{x}_{k-1}+bu_{k}$

$p_{k}=ap_{k-1}a+q$

更新過程：

$g_{k}=p_{k}h/\left ( hp_{k}h+r \right )$

$\hat{x}_{k}=\hat{x}_{k}+g_{k}\left ( z_{k}-h\hat{x}_{k} \right )$

$p_{k}=\left ( 1-g_{k}h \right )p_{k}$

上面的公式適合一維變量的卡爾曼濾波，將變量擴展到多維，用向量和矩陣替換上面的變量，就可以實現多維變量的卡爾曼濾波，下面的公式適用於多維變量。

預測過程：

$\hat{x}_{k}=A\hat{x}_{k-1}+Bu_{k}$

$P_{k}=AP_{k-1}A^{T}+Q$

更新過程：

$G_{k}=P_{k}H^{T}\left ( HP_{k}H^{T}+R \right )^{-1}$

$\hat{x}_{k}=\hat{x}_{k}+G_{k}\left ( z_{k}-H\hat{x}_{k} \right )$

$P_{k}=\left ( 1-G_{k}H \right )P_{k}$

二、實現代碼

下面我們通過c++代碼來實現卡爾曼濾波算法，所實現的算法爲一維濾波算法。首先定義卡爾曼濾波的參數

typedef struct{
    float filterValue;//濾波後的值
    float kalmanGain;//Kalamn增益
    float A;//狀態矩陣
    float H;//觀測矩陣
    float Q;//狀態矩陣的方差
    float R;//觀測矩陣的方差
    float P;//預測誤差
    float B;
    float u;
}KalmanInfo;

下面是卡爾曼濾波器的初始化函數，在這個函數中，info爲卡爾曼濾波參數的指針。初始化的參數是針對一個車速濾波過程的設置。

void Kalm::initKalmanFilter(KalmanInfo *info)
{
    info->A = 1;
    info->H = 1;
    info->P = 0.1;
    info->Q = 0.05;
    info->R = 0.1;
    info->B = 0.1;
    info->u = 0;
    info->filterValue = 0;
}

卡爾曼濾波過程函數，函數的輸入info爲卡爾曼濾波參數的指針，new_value爲新的測量值，函數返回濾波後的估計值。

float Kalm::kalmanFilterFun(KalmanInfo *info, float new_value)
{
    float predictValue = info->A*info->filterValue+info->B*info->u;//計算預測值
    info->P = info->A*info->A*info->P + info->Q;//求協方差
    info->kalmanGain = info->P * info->H /(info->P * info->H * info->H + info->R);//計算卡爾曼增益
    info->filterValue = predictValue + (new_value - predictValue)*info->kalmanGain;//計算輸出的值
    info->P = (1 - info->kalmanGain* info->H)*info->P;//更新協方差
    return info->filterValue;
}

三、示例

下面我們通過是一個車速濾波的示例來體驗卡爾曼濾波的效果。通過上面的介紹，R對濾波效果的影響比較大，在這個示例中，我們分別將R取爲0.1和0.5，來看一下車速的濾波效果。首先R取爲0.1時，濾波效果如下圖所示。其中，藍色線爲濾波前的車速，紅色線爲濾波後的車速。從圖中可以看到濾波後的信號與濾波前的信號跟隨很好，滯後很小。基本波動被濾掉了，但也帶入了一些波動。

下圖爲R取爲0.5時的濾波效果，很明顯，這張圖信號的跟隨效果比上圖要差，滯後也多，但是濾波後曲線更平滑。

濾波算法（四）—— 卡爾曼濾波算法

一、算法介紹

1、狀態估計

2、卡爾曼增益

3、完整卡爾曼濾波算法

二、實現代碼

三、示例

如何實現ubuntu虛擬機與windows系統之間複製粘貼

排序算法（三）—— 插入法排序算法

控制算法（一）—— PID控制算法

毫米波雷達介紹

如何用少線束的激光雷達獲得多線束的激光雷達的感知效果——點雲配準

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

linux以太網驅動總結