GNSS算法进阶（二）- kalman滤波单点定位算法代码实现

状态预测

kalman滤波与前面实现的最小二乘最大的区别就在于，kalman滤波用到了历史状态信息，即进行状态预测。

公式中BU(k)（控制输入）在我们定位模型中用不到。

从(k-1)时刻将状态转移到(k)时刻的转移矩阵，其实就是我们初中学的位置、速度、加速度之间的关系，只是初中学的是一维的，现在是ecef框架下的三维。

除了预测的状态量，我们还需要知道预测的状态量的方差协方差信息。学过平差中的误差传播定律，那么预测的协方差更新也很容易理解。

只是需要多加一个过程噪声矩阵Q，来放大预测状态的方差协方差信息。

直观上也很容易理解，任何模型都有误差，为了防止预测的状态权重过大，增加过程噪声以降低后续加权平均中预测状态的影响。

状态以及其相应方差协方差的预测在以下函数中实现

/* temporal update of position/velocity/acceleration */
static void udpos_spp(rtk_t *rtk, double tt)

我们实现的是PVA模型，也即位置/速度/加速度模型。

函数的输入tt是相邻两次状态转移的时间间隔，主要是过程噪声的计算需要依赖tt。直观上也很容易理解，外推时间越长，过程噪声设置应该越大。

---

测量更新

①残差计算

kalman滤波残差计算和最小二乘的类似，最小二乘中计算的是基于概略位置的修正值，kalman滤波也一样，参考公式(4)，也是相对于预测状态的更新量。区别在于kalman滤波算法的状态量多了速度和加速度，虽然并没有观测值对他们进行更新。

设计矩阵和残差阵依然采用最小二乘的rescode函数，只是将其中接收机所在位置进行了更新，并重新命名为

/* pseudorange residuals -----------------------------------------------------*/ 
static int rescode_mulfreq_filter(int iter, const obsd_t *obs, int n, const double *rs, const double *dts, const double *vare, const int *svh, const nav_t *nav, const double *x, const prcopt_t *opt, double *v, double *H, double *var, double *azel, int *vsat, double *resp, int *ns)

函数中并未考虑速度和加速度状态量，所以在此函数之后，增加了一个专门插入这俩状态量的函数。

static void insert_vel_acc_colums(const int nv, double *H)

原先考虑与最小二乘公用一个函数，后续有机会再统一吧。

②滤波

kalman滤波算法逻辑，与rtk算法中完全一样。直接复用filter函数，filter函数中调用filter_，调用之前做了一步处理，即仅状态值非0且方差大于0的状态参与滤波。这样处理，是因为在rtk中默认的状态量包含了所有卫星的模糊度，对于没有观测值更新的模糊度明显是不需要参与滤波的。 所以在使用该filter时，注意各状态量的初值不要设置为0。或者直接调用filter_函数。

将滤波结果更新回rtk_t->x_spp和rtk_t->P_spp中，便于下历元滤波。同时将滤波存储到sol_t中。

---

/* kalman filter ---------------------------------------------------------------
* kalman filter state update as follows:
*
*   K=P*H*(H'*P*H+R)^-1, xp=x+K*v, Pp=(I-K*H')*P
*
* args   : double *x        I   states vector (n x 1)
*          double *P        I   covariance matrix of states (n x n)
*          double *H        I   transpose of design matrix (n x m)
*          double *v        I   innovation (measurement - model) (m x 1)
*          double *R        I   covariance matrix of measurement error (m x m)
*          int    n,m       I   number of states and measurements
*          double *xp       O   states vector after update (n x 1)
*          double *Pp       O   covariance matrix of states after update (n x n)
* return : status (0:ok,<0:error)
* notes  : matirix stored by column-major order (fortran convention)
*          if state x[i]==0.0, not updates state x[i]/P[i+i*n]
*-----------------------------------------------------------------------------*/
static 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)

结果分析

为了防止采样时间过久，导致状态预测权重过低没有滤波效果，特意选了一个采样频率1s的数据。该数据下载自香港cors数据。（ftp://ftp.geodetic.gov.hk/rinex3）

相关的数据配置以及代码，已经在以下两个commit更新

55df872b4de9362e1166bb46ab9a6b2df131e8d0

7569205e5f6df464e10fa12a29f569e672cb7b44

如在上上推文中最后所描述的，加上kalman滤波的结果并没有想象中的平滑，初步怀疑是状态预测的精度太低。

考虑到静态的基站数据，速度均为零，所以在代码中直接增加三方向速度为0的约束，约束代码如下，非常简单

        /* add velocity constrain of static mode */
        for (j = 3; j < 6; j++)
        {
            for (k = 0; k < NX_F; k++) {
                H[k + nv * NX_F] = 0.0;
            }
            H[j + nv * NX_F] = 1.0;
            v[nv] = 0.0 -x[j] ;
            var[nv] = SQR(0.001);
            nv++;
        }

增加零速约束前后对比如下，其中噪声较大的结果为无零速约束的kalman滤波结果，平滑结果为增加零速约束结果。

增加零速约束的结果相当平滑，定位精度在分米量级，精度超出预期。这也从层面在一定层度上表明我们增加的kalman滤波代码没有问题。

​

公众号

有时会将代码 或者资源放在个人公众号上，有问题，在公众号后台回复，也回答的比较快一些，欢迎关注 GNSS和自动驾驶

其他相关文章链接汇总

GNSS算法学习系列教程 - 文章列表_梧桐Fighting的博客-CSDN博客