GPS從入門到放棄（二十五） --- 卡爾曼濾波

GPS從入門到放棄（二十五） — 卡爾曼濾波

概述

單點定位的結果因爲是單獨一個點一個點進行的，所以連續起來看數據可能出現上串下跳的情況，事實上並不符合實際情況。爲了解決這個問題，考慮到物體運動的連續性和運動變化的緩慢性，可以通過濾波器來平滑位置軌跡。
濾波器的設計需要對物體的運動做一些理性的、常規的假設，比如要符合牛頓運動定律等。這裏最常用的濾波器就是卡爾曼濾波器。

卡爾曼濾波器用來解決用線性微分方程描述的離散時間控制過程中的狀態估計問題。其目標是使系統狀態的估計值有最小均方誤差。卡爾曼濾波器來源於匈牙利數學家卡爾曼的博士論文，推導過程這裏就不多贅述了，在諸多文獻中都有講解，我們直接講怎麼用。

對於一個線性離散系統，設其系統方程爲：
$\boldsymbol{x}_k = \boldsymbol{A}\boldsymbol{x}_{k-1}+\boldsymbol{B}\boldsymbol{u}_{k-1}+\boldsymbol{w}_{k-1}$
測量方程式爲：
$\boldsymbol{y}_k = \boldsymbol{H}\boldsymbol{x}_{k}+\boldsymbol{v}_{k}$
其中隨機變量 $\boldsymbol{w}_k$ 和 $\boldsymbol{v}_k$ 分別表示過程噪聲與測量噪聲。假設它們是相互獨立的正態分佈的白噪聲，且其協方差陣分別爲 $\boldsymbol{Q}$ 和 $\boldsymbol{R}$。（要注意的是，實際應用中，$\boldsymbol{A}$，$\boldsymbol{B}$，$\boldsymbol{H}$，$\boldsymbol{Q}$ 和 $\boldsymbol{R}$ 都可能隨着時間變化）。

對於這個系統的卡爾曼濾波的過程可分爲兩步：預測和校正。

• 預測

預測是用上一時刻的狀態值通過狀態方程來估計這一時刻的狀態值，同時還要估計誤差協方差陣，因爲此時狀態值還沒有經過校正，所以稱其爲先驗值，用上標 ${}^-$ 來表示。預測方程爲：
$\begin{array}{l} \hat{\boldsymbol{x}}_k^- = \boldsymbol{A}\hat{\boldsymbol{x}}_{k-1}+\boldsymbol{B}\boldsymbol{u}_{k-1}\\ \boldsymbol{P}_k^- = \boldsymbol{A}\boldsymbol{P}_{k-1}\boldsymbol{A}^T+\boldsymbol{Q} \end{array}$

其中符號 $\hat{}$ 表示估計值，$\boldsymbol{P}$ 爲系統狀態值的誤差協方差陣。

• 校正

校正是先計算卡爾曼增益，然後用測量值及狀態預測值根據卡爾曼增益來更新狀態並更新誤差協方差陣。校正方程爲：
$\begin{array}{l} \boldsymbol{K}_g = \boldsymbol{P}_k^-\boldsymbol{H}^T(\boldsymbol{HP}_k^-\boldsymbol{H}^T+\boldsymbol{R})^{-1}\\ \hat{\boldsymbol{x}}_k = \hat{\boldsymbol{x}}_k^-+\boldsymbol{K}_g(\boldsymbol{y}_k-\boldsymbol{H}\hat{\boldsymbol{x}}_k^-)\\ \boldsymbol{P}_k = (\boldsymbol{I}-\boldsymbol{K}_g\boldsymbol{H})\boldsymbol{P}_k^- \end{array}$

其中 $\boldsymbol{K}_g$ 爲卡爾曼增益，$\boldsymbol{I}$ 爲單位陣。

卡爾曼濾波器有幾個優點：

1. 卡爾曼濾波器不僅提供了狀態更新值，同時還提供了其誤差協方差，這使得其可根據測量值及誤差協方差自動調節卡爾曼增益，以達到最優估計。卡爾曼增益的動態變化通俗來解釋就是，如果預測值比較靠譜（誤差協方差小），則濾波結果多相信預測值一些；反之，若測量值比較靠譜（$\boldsymbol{R}$ 比較小），則濾波結果多相信測量值一些。
2. 相對於最小二乘法來說，卡爾曼濾波器可以遞推計算，有測量值即可更新，不用等到所有測量都結束才能計算。

當然卡爾曼濾波器應用起來也有其要求：過程噪聲方差 $\boldsymbol{Q}$、測量噪聲方差 $\boldsymbol{R}$ 必須要準確。所以其設計調試的重難點就是對過程噪聲方差 $\boldsymbol{Q}$、測量噪聲方差 $\boldsymbol{R}$ 的準確估計。

精密單點定位中的卡爾曼濾波

RTKLIB 中的精密單點定位就用了卡爾曼濾波法，其狀態變量包含接收機位置、接收機速度、接收機鐘差、對流層參數以及每顆衛星的載波偏移，其中載波偏移包含周整模糊度及小數部分。

在應用卡爾曼濾波的過程中，$\boldsymbol{A}$ 爲單位陣，$\boldsymbol{B}$ 爲零矩陣，代碼中調用了 filter 函數進行卡爾曼濾波，代碼如下。

filter 函數首先進行狀態預測，然後調用 filter_ 函數進行校正。這個過程代碼很直接，與公式一一對應，唯一需要注意的就是代碼裏的H是公式中的H的轉置。

int filter(double *x, double *P, const double *H, const double *v,
                  const double *R, int n, int m)
{
    double *x_,*xp_,*P_,*Pp_,*H_;
    int i,j,k,info,*ix;

    ix=imat(n,1); for (i=k=0;i<n;i++) if (x[i]!=0.0&&P[i+i*n]>0.0) ix[k++]=i;
    x_=mat(k,1); xp_=mat(k,1); P_=mat(k,k); Pp_=mat(k,k); H_=mat(k,m);
    for (i=0;i<k;i++) {
        x_[i]=x[ix[i]];
        for (j=0;j<k;j++) P_[i+j*k]=P[ix[i]+ix[j]*n];
        for (j=0;j<m;j++) H_[i+j*k]=H[ix[i]+j*n];
    }
    info=filter_(x_,P_,H_,v,R,k,m,xp_,Pp_);
    for (i=0;i<k;i++) {
        x[ix[i]]=xp_[i];
        for (j=0;j<k;j++) P[ix[i]+ix[j]*n]=Pp_[i+j*k];
    }
    free(ix); free(x_); free(xp_); free(P_); free(Pp_); free(H_);
    return info;
}

int filter_(const double *x, const double *P, const double *H,
                   const double *v, const double *R, int n, int m,
                   double *xp, double *Pp)
{
    double *F=mat(n,m),*Q=mat(m,m),*K=mat(n,m),*I=eye(n);
    int info;

    matcpy(Q,R,m,m);
    matcpy(xp,x,n,1);
    matmul("NN",n,m,n,1.0,P,H,0.0,F);       /* Q=H'*P*H+R */
    matmul("TN",m,m,n,1.0,H,F,1.0,Q);
    if (!(info=matinv(Q,m))) {
        matmul("NN",n,m,m,1.0,F,Q,0.0,K);   /* K=P*H*Q^-1 */
        matmul("NN",n,1,m,1.0,K,v,1.0,xp);  /* xp=x+K*v */
        matmul("NT",n,n,m,-1.0,K,H,1.0,I);  /* Pp=(I-K*H')*P */
        matmul("NN",n,n,n,1.0,I,P,0.0,Pp);
    }
    free(F); free(Q); free(K); free(I);
    return info;
}