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;
}