GPS从入门到放弃（二十五）、卡尔曼滤波

一、概述

单点定位的结果因为是单独一个点一个点进行的，所以连续起来看数据可能出现上串下跳的情况，事实上并不符合实际情况。为了解决这个问题，考虑到物体运动的连续性和运动变化的缓慢性，可以通过滤波器来平滑位置轨迹。
滤波器的设计需要对物体的运动做一些理性的、常规的假设，比如要符合牛顿运动定律等。这里最常用的滤波器就是卡尔曼滤波器。

卡尔曼滤波器用来解决用线性微分方程描述的离散时间控制过程中的状态估计问题。其目标是使系统状态的估计值有最小均方误差。卡尔曼滤波器来源于匈牙利数学家卡尔曼的博士论文，推导过程这里就不多赘述了，在诸多文献中都有讲解，我们直接讲怎么用。

对于一个线性离散系统，设其系统方程为：

二、精密单点定位中的卡尔曼滤波

RTKLIB 中的精密单点定位就用了卡尔曼滤波法，其状态变量包含接收机位置、接收机速度、接收机钟差、对流层参数以及每颗卫星的载波偏移，其中载波偏移包含周整模糊度及小数部分。

在应用卡尔曼滤波的过程中，A 为单位阵，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;
}