状态预测
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和自动驾驶