企业官网建设流程全解析

https://blog.csdn.net/weixin_40681914/article/details/155562675?spm=1001.2014.3001.5501

二. 速度LSQ算法

上篇文章主要介绍了PVT中位置信息的计算过程，在车载导航定位过程中，速度信息的迭代计算也是一个很重要的部分，依据伪距和多普勒的关系式：

可以得到

其中，电离层、对流层误差的变化相对较小，可以舍去，接收机和用户之间的距离变化率可以描述为速度变化在卫地距单位向量上的投影，即：

那么多普勒定速公式可以变成：

（1）直接求解方法

直接求解法认为多普勒方程是一个关于接收机速度的方程，普勒定速方程中的未知数是接收机速度和接收机钟漂，对方程进行整理：

即：

可以变种成为HX=L，也可以根据其列出多个卫星的联合方程，其中e就直接用（ex,ey,ez）表示，即：

注意点：

a：不同厂家上报的多普勒的符号会有区别，在做解算时需要注意是否需要取反；

b：多普勒频移（Hz）和多普勒（m/s）之间的数学关系；

c：多普勒和伪距关于最小二乘解算的的推导公式基本一致，也可以使用多普勒残差进行定权剔星以及基于高精反算查看计算的正确性；

（2）仿照伪距求解线性化

仿照伪距解算方程，即使用（vx0，vy0，vz0，clkdritt0）将该多普勒定速方程展开，即：

整合即：

记多普勒残差为：，等式右侧为HX，该公式即与伪距残差公式基本一致；RTKLIB也是采用这种方式，其初始化点为（0,0,0,0），也可以理解为直接计算的一种方式，下面是RTKLIB中关于多普勒更新速度的函数，博主也增加了部分注释，便于理解！

//RTKLIB代码 /* 计算多普勒残差函数 doppler residuals ---------------------------------------------------------*/ static int resdop(const obsd_t *obs, int n, const double *rs, const double *dts, const nav_t *nav, const double *rr, const double *x, const double *azel, const int *vsat, double *v, double *H) { double lam,rate,pos[3],E[9],a[3],e[3],vs[3],cosel; int i,j,nv=0; trace(3,"resdop : n=%d\n",n); ecef2pos(rr,pos); xyz2enu(pos,E); for (i=0;i<n&&i<MAXOBS;i++) { lam=nav->lam[obs[i].sat-1][0]; //判断观测量是否可用,卫星速度是否可用 if (obs[i].D[0]==0.0||lam==0.0||!vsat[i]||norm(rs+3+i*6,3)<=0.0) { continue; } //ENU坐标系下的视线向量，可以通过绘制坐标系下的向量推导 /* line-of-sight vector in ecef */ cosel=cos(azel[1+i*2]); a[0]=sin(azel[i*2])*cosel; a[1]=cos(azel[i*2])*cosel; a[2]=sin(azel[1+i*2]); matmul("TN",3,1,3,1.0,E,a,0.0,e); //卫星相对于接收机的运行速度 /* satellite velocity relative to receiver in ecef */ for (j=0;j<3;j++) vs[j]=rs[j+3+i*6]-x[j]; //多普勒残差公式中关于卫星相对于接收机的运行速度在视距方向的投影 //地球自转影响; //即上述公式中的（vsat-vrcv）*e,具体e中的负号要与vrcv是减数还是被减数相关 /* range rate with earth rotation correction */ rate=dot(vs,e,3)+OMGE/CLIGHT*(rs[4+i*6]*rr[0]+rs[1+i*6]*x[0]- rs[3+i*6]*rr[1]-rs[ i*6]*x[1]); //计算多普勒残差，即上述公式中的p-p0 /* doppler residual */ v[nv]=-lam*obs[i].D[0]-(rate+x[3]-CLIGHT*dts[1+i*2]); //计算H矩阵 /* design matrix */ for (j=0;j<4;j++) H[j+nv*4]=j<3?-e[j]:1.0; nv++; } return nv; } /* estimate receiver velocity ------------------------------------------------*/ static void estvel(const obsd_t *obs, int n, const double *rs, const double *dts, const nav_t *nav, const prcopt_t *opt, sol_t *sol, const double *azel, const int *vsat) { double x[4]={0},dx[4],Q[16],*v,*H; int i,j,nv; trace(3,"estvel : n=%d\n",n); v=mat(n,1); H=mat(4,n); //迭代解次数 for (i=0;i<MAXITR;i++) { //计算可用卫星的多普勒残差、H阵 /* doppler residuals */ if ((nv=resdop(obs,n,rs,dts,nav,sol->rr,x,azel,vsat,v,H))<4) { break; } //最小二乘计算公式 /* least square estimation */ if (lsq(H,v,4,nv,dx,Q)) break; //迭代解更新速度值 for (j=0;j<4;j++) x[j]+=dx[j]; //迭代结束条件以及速度赋值 if (norm(dx,4)<1E-6) { for (i=0;i<3;i++) sol->rr[i+3]=x[i]; break; } } free(v); free(H); }

三 . kalman滤波算法

3.1 简单介绍

卡尔曼滤波的本质是 “传感器数据融合器” 和 “状态最优估计器”的融合，最终输出一个介于模型预测和传感器测量之间的、概率上最优的估计值。。卡尔曼滤波解决的核心问题是：如何从一系列包含噪声的观测数据中，动态地、最优地估计出一个无法直接测量但又在变化的系统的真实状态？其主要分为两个部分：预测和更新模块。预测是根据系统设置的物理模型，从上一时刻的状态量预测、估计当前时刻的状态量（预测估计本身具有一定的不确定性）；更新是利用当前时刻的传感器测量值来修正这个预测（测量值也带有不确定性）。以课本上经常提到的温度计测量环境温度来讲，环境温度为一个状态量，温度计的度数为一个量测量，但是温度计的测量精度本身就是一个不确定性。

状态方程：

Xk、Xk-1：k和k-1时刻的系统真实状态，可以理解为未知数，比如gnss定位中的位置、速度信息、INS估计中的姿态信息；

：状态转换矩阵，由物理定律决定，如k时刻的位置 = k-1时刻的位置 + k-1时刻的速度 × 时间间隔+1/2 ×k-1时刻的加速度 × 时间间隔 × 时间间隔；

Bk * Uk：其他控制输入，是影响但当前状态的另一输入，当前GNSS解算中没有使用到这一输入；

wk：状态量的过程噪声，代表模型误差。服从均值为0，协方差为 Q_k 的高斯分布，GNSS解算中可以理解为位置、速度的误差估计范围；
观测方程：

Zk：k时刻的传感器观测值，GNSS中如观测量，GNSS/INS融合中的GNSS位置、速度等信息。

Hk：量测矩阵，描述如Zk和Xk之间的数学关系，GNSS中可以依据上篇中的伪距残差和位置变化量的关系L=HX获得H矩阵；

Vk：观测噪声，表示传感器在量测过程中的误差，服从均值为0，协方差为 R_k 的高斯分布，GNSS解算中可以根据定权对该部分噪声进行设置；

卡尔曼滤波方程的5个重点公式如下：

（1）预测估计方程

一步状态估计方程，描述前后时刻的状态关系：

一步协方差估计方程，其描述的是预测的不确定性（根据协方差传播计算规律）以及状态估计本身引入的不确定误差Q；

上述两个方程都是根据k-1估计k时刻，那就是说在整个滤波初始时，状态量的初始值设置非常重要，即需要用X0估计X1，P0估计P1。

（2）量测更新方程

增益方程，重点方程，其可以理解为一个开关，描述的是更相信预测还是更相信量测；

该数值严重影响最终的计算结果，所以在此需要描述Q和R对于K影响的关系：

状态更新方程，是最终状态量的输出结果，通过K来控制相信预测还是量测，描述的是实际量测量和预计观测量之间的误差，称之为新息；

协方差更新方程，为最终协方差的输出结果；

3.2 PVT kalman滤波方程

假设PVT中的状态量设置为位置误差、速度误差、钟差误差、钟漂误差，量测量为伪距残差信息，多普勒残差信息，根据伪距解算公式、多普勒解算公式、位置速度公式，钟差钟漂公式，仿照上述状态方程和观测方程可以写成：

状态方程：

其中，I矩阵为单位阵，O矩阵为全0矩阵。

量测方程：

其中，e矩阵为卫星视距向量，I1为钟差相关的单位阵，与各个系统相关，以8个卫星观测量为例，列出e矩阵和I1矩阵的表达，gps1和gps2分别代表gps系统的两个卫星；

从上述表达来看，计算较为复杂，所以一般情况下会将伪距+钟差作为一个卡尔曼滤波流程，速度+钟漂作为一个卡尔曼滤波流程，则整个公式细化为下面的两种方式。

（1）位置+钟差

PVT中的状态量为位置误差、单系统/四系统的钟差信息误差，量测量为伪距残差信息。

a：假设只有6个BDS卫星观测量，则只需要估计1个接收机钟差，则状态方程和量测方程为：

即

b：假设4系统分别有2个观测量，则只需要估计4个接收机钟差，则状态方程和量测方程为：

（2）速度+钟漂

PVT中的状态量为速度误差、钟漂误差，量测量为多普勒残差信息。

假设有6个卫星观测量，则状态方程和量测方程为：

同位置计算一样，即

（3）延伸——>位置+速度+加速度

定位解算中状态量为位置误差、速度误差、加速度误差、钟差、钟漂信息，量测量为伪距残差和多普勒残差信息。

状态方程：

量测方程：

3.3 参数设置

从kalman标准方程可以看到两个重点参数设置，Q阵和R阵。Q阵为过程噪声协方差矩阵，描述是状态量的噪声水平；R阵为观测噪声协方差矩阵，描述的是观测量的噪声水平，在上述表述中已经具体列出Q/R的设置对于整个结果的影响。本文在此描述下GNSS 位置、速度解算过程中的两个矩阵设置。

（1）观测噪声协方差矩阵R阵

R阵在此描述的是伪距和多普勒的观测量噪声，代表了这两者数据的不确定性，在此假设各个卫星之间互不影响，此时R阵为一个对角阵，即：

其具体的参数设置可以参考LSQ中的定权模块，比如引入高度角、CNR、lock_t、残差信息量、系统、频点、电离层、URA、多路径等计算量进行定权，简单意义上来讲可以理解为高CNR的卫星跟踪精度比较好，方差量就会比较小，但是该过程是一个自适应调整的过程，并且在整个迭代计算期间，需要及时调整R阵，为了降低计算过程中的复杂度，经常会对其进行归一化处理。

比如车载环境中，空旷、遮挡和严重遮挡的情况下，可能对于CNR、高度角、多径的门限或者定权参数都不一样，并不是一个写死的定权方法。

假设在kalman滤波过程中个别卫星伪距残差较大，需要降低该卫星的影响，应该如何操作？

基于新息量降低异常对整个算法的影响，比如杨元喜老师提出的IGG-III方法。

（2）状态估计过程噪声Q阵

Q阵在此描述的是状态预测模型的不确定性，假设状态量之间无任何数学关联，其与R阵一样，也会为一个对角阵，但是在GNSS导航滤波使用过程中，Qpos会受到Qvel的影响，Qvel会受到Qacc的影响，一般常用该模型表示：

其中，qa（m/s3）是加速度误差过程噪声的误差，Qc为时钟模块的协方差阵，与pos-vel影响一致，也是采用误差传递的模型建立：

qe(m2/s)为钟差误差过程噪声的方差值，qf(m3/s)为钟漂误差噪声的方差值；

一般情况下也会根据当前的运动状态设置不同的qa，并且一般xy向和z向的数值设置也有可能存在区别，假定状态量此时是位置误差、速度误差、钟差误差、钟漂误差，即整理上述公式写出细化Q阵，即：

（默认四系统之间的钟差互相独立）

常见的参数设计：

一般设置中，GLONASS与其他系统设置存在一定区别；qe和qf一般和晶振精度相关。

问题1：滤波过程中结果抖动较大怎么办？

滤波滤波，应为一个较为缓变的过程，出现这种问题的原因一般都是较大程度上相信量测量，此时可以降低量测量的影响，使其更相信估计量，即增大R的设置，使P变小，K变小，更相信估计值。

问题2：滤波过程中结果过于平滑怎么办？

与问题1类似，其主要原因还是较为相信预测值，此时应该更相信量测值，即减小R，P变大、K变大、更相信两侧至。

问题3：滤波器严重发散怎么办？
发散可以通过Xk这一公式来体现，分为两个方面，一是K值增大较快（P阵异常），二是新息差增大较快；P阵异常可以通过检查Q和R阵设置发现，新息差值异常可以通过检查异常残差、定权异常、几何分布发现，但是出现该问题的时候，在实时运行界面主要的工作是检测-诊断-恢复机！可以在代码中增加关于P阵对角参数、最终计算参数以及质量指标的的严格限制！

问题4：卡尔曼是序贯算法？LSQ是直接相乘算法？

一是由于计算效率问题，多个卫星一次性处理的时候数据操作量较大，矩阵求逆时间较长，二是由于滤波过程中本身就是一个平滑处理的过程，可以在每次迭代后对于卫星的定权重新做调整。

RTKLIB代码部分：

/* 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) { 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; } extern 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; }