当前位置:网站首页>C code implementation of robust estimation in rtklib's pntpos function (standard single point positioning spp)
C code implementation of robust estimation in rtklib's pntpos function (standard single point positioning spp)
2022-07-06 12:53:00 【Proletarians】
One 、 background
stay rtklib in spp The location algorithm is mainly in pntpos Function ,pvt stay pntpos The realization is incisive .
1. In terms of the types of observations :
(1) If we have pseudo range observations , So it can be estpos To analyze the stability of positioning and receiver clock error ;(2) If we still have Doppler shift , So it can be estvel The constant speed , Of course , If we have carrier observations , The speed can also be fixed through the carrier difference between epochs .
2. In terms of the frequency of observations :
(1) If we are double frequency pseudo range observations , Then according to our different treatment strategies for the ionosphere , The types of observations used are also different , say concretely , If we use the ionospheric combination , Then we need to use the double frequency observations to combine the pseudorange de ionosphere to get the combined observations ; If we use brdc Deal with ionospheric delays , Then we only use L1 Pseudo range observations , If we use TEQC Conduct data quality analysis , You will find L1 Than L2 Of mp1 To be less than mp2, in addition , We're doing rtk When , The calculation of the number of effective satellites is also based on L1 To accumulate .
3. Tolerance spp The necessity of :
Why spp Robust estimation of ? Because when we carry out high-precision positioning ,spp It's for kalman filter initial value , If the initial value is not correct , Then we pass filter Got float The solution deviates from the actual position , Will cause fix The latter solution is also wrong , For example, the phenomenon of flying points .
4. spp Introduction to mathematical model :
In the grasp of spp Based on the mathematical model of , The mathematical model consists of a functional model and a stochastic model , The function model is the pseudo range observation equation , Stochastic models include prior stochastic models ( Equal weight model 、 Height angle random model 、 Signal noise ratio random model ) And a posteriori random model (helmet Variance component estimation ), The function model allows us to get the error equation v=Bx-l, The random model gives us the weight matrix P. So in rtklib Of spp in , Is obtained by using weighted iterative least squares dx, And then with the initial value x0 Add to get pos and dt value .
5.spp Random models :
About stochastic models , Want to say more , If a friend wants to use helmet Variance component estimation , Then we should ensure that the priori weight matrix is as accurate as possible , Otherwise, it's nonsense , Actually in rtklib in varerr Weight ratio in function , We can use helmet To get , for example ,gps and bds stay spp Is the middle weight the same , Even in bds in ,geo/meo/igso Is the weight of the same ? You can practice it . in addition ,rtklib In the height angle random model sin(el) There is no square , have access to SQR(sin(el)). Speaking of this , I have a supplementary , The variance of observation error is composed of three parts , Satellite position variance ( Satellite side ), Atmospheric delay variance ( The communication end , the ionosphere 、 The troposphere ), The measurement error obtained by using the random model ( Receiver end , That is, where random models are used ).
Two 、 Tolerance spp Realization
stay rtklib in , Epoch k There are many iterations , The maximum number of iterations is 10, Even in the case of first positioning , That is, the state vector ( With unknown vector 、 Unknown parameter means ) All for 0 When , It just needs 6 The final result can be obtained in one iteration , Subsequent epochs generally require 3 Times to get the final result , Yes, of course , The result may be incorrect , This is the time valpos The function works ,valpos Mainly through chi square test and dop Value test is used to test the effectiveness , If it fails , Then the result of the epoch will not be output , Some friends may not notice this , Your focus may be lsq More .
2.1. Robust estimation code
rtklib in spp The code of robust estimation applied on is as follows , I first find the largest value through the standardized residual to eliminate , I only have zero rights and guaranteed rights , Specific application data, and then modify the details . The code is as follows :
/* compare residual data -------------------------------------------------*/
static int cmpres(const void *p1, const void *p2)
{
double *q1 = (double *)p1, *q2 = (double *)p2;
double delta = *q1 - *q2;
return delta<-0.0 ? -1 : (delta>0.0 ? 1 : 0);
}
/* estimate receiver position ------------------------------------------------*/
static int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
const double *vare, const int *svh, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
double *resp, char *msg)
{
double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*v_,*var,sig;
int i,j,k,info,stat,nv,ns;
double k0=8.0;
int ref = 0; // robust estimate start flag
trace(3,"estpos : n=%d\n",n);
v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1);
v_ = mat(n + 4, 1); // deal with residual(derive from [v])
for (i=0;i<3;i++) x[i]=sol->rr[i];
for (i=0;i<MAXITR;i++) {
/* pseudo range residuals */
nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp,
&ns);// H Is in accordance with the nv*NX Arranged , namely nv For the number of lines ,NX Represents the number of columns , Constant for the 7
matcpy(v_, v, ns, 1);
// predicted normalized residual
qsort(v_, ns, sizeof(double), cmpres);
if (fabs(v_[0] < 100 && fabs(v_[ns - 1]) < 100))
{
ref = 1;
}
if (ref == 1) // robust estimate start
{
matcpy(v_, v, ns, 1);
for (j = 0; j < ns; j++)
{
v_[j] = fabs(v_[j]) / sqrt(var[j]);
}
qsort(v_, ns, sizeof(double), cmpres);
// local variable
int v_max_index=0; // the biggest normalized residual index
double v_max= v_[ns - 1]; // the biggest normalized residual value
matcpy(v_, v, ns, 1);
for (j = 0; j < ns; j++)
{
v_[j] = sqrt(SQR(v_[j]) / var[j]);
if (fabs(v_[j] -v_max)<1e-9)
{
v_max_index = j;
break;
}
}
if (v_max > k0)
{
if (ns > 4)
{
v[v_max_index] = 0;
for (k = 0; k < NX; k++) H[k + v_max_index*NX] = 0;
}
}
}
if (nv<NX) {
sprintf(msg,"lack of valid sats ns=%d",nv);
break;
}
/* weight by variance */
for (j=0;j<nv;j++) {
sig=sqrt(var[j]);
v[j]/=sig;
for (k=0;k<NX;k++) H[k+j*NX]/=sig;
}
/* least square estimation */
if ((info=lsq(H,v,NX,nv,dx,Q))) {
sprintf(msg,"lsq error info=%d",info);
break;
}
for (j=0;j<NX;j++) x[j]+=dx[j];
if (norm(dx,NX)<1E-4) {
sol->type=0;
sol->time=timeadd(obs[0].time,-x[3]/CLIGHT);
sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */
sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */
sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */
sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */
for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0;
for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX];
sol->qr[3]=(float)Q[1]; /* cov xy */
sol->qr[4]=(float)Q[2+NX]; /* cov yz */
sol->qr[5]=(float)Q[2]; /* cov zx */
sol->ns=(unsigned char)ns;
sol->age=sol->ratio=0.0;
/* validate solution */
if ((stat=valsol(azel,vsat,n,opt,v,nv,NX,msg))) {
sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE;
}
free(v); free(H); free(var);
return stat;
}
}
if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i);
free(v); free(H); free(var);
return 0;
}
2.2. The test data
Use a piece of data from the self-test , Strong signal , Static data measured in the open sky , The information in the header file is as follows :
I'm in the calendar [2020 6 5 9 8 14.0000000] Yes G09 Satellite pseudo range observations are applied 100m Gross error of , by 21954773.950 Turned into 21954873.950.
Robust estimation is not used , The result obtained directly from the iterative least squares is :
Then if robust estimation is used, the result is :
The first two epochs s There is no gross error pollution in the observed value , Its spp The results are :
It can be seen that the resistance is poor spp It has good reliability and positioning accuracy .
// Updated on 2020.11.27 01:36:00
边栏推荐
- 基本Dos命令
- FairyGUI复选框与进度条的组合使用
- rtklib单点定位spp使用抗差估计遇到的问题及解决
- Agile development helps me
- Office prompts that your license is not genuine pop-up box solution
- [algorithm] sword finger offer2 golang interview question 8: the shortest subarray with a sum greater than or equal to K
- MySQL error warning: a long semaphore wait
- Latex learning
- Matlab读取GNSS 观测值o文件代码示例
- [algorithm] sword finger offer2 golang interview question 2: binary addition
猜你喜欢
Fairygui joystick
SVN更新后不出现红色感叹号
First use of dosbox
地球围绕太阳转
Liste des boucles de l'interface graphique de défaillance
Fairygui gain buff value change display
Derivation of logistic regression theory
Detailed explanation of balanced binary tree is easy to understand
FairyGUI循環列錶
Unity3d, Alibaba cloud server, platform configuration
随机推荐
SSD technical features
[algorithm] sword finger offer2 golang interview question 9: subarray with product less than k
[算法] 剑指offer2 golang 面试题9:乘积小于k的子数组
[leetcode622] design circular queue
抗差估计在rtklib的pntpos函数(标准单点定位spp)中的c代码实现
PR 2021 quick start tutorial, first understanding the Premiere Pro working interface
The service robots that have been hyped by capital and the Winter Olympics are not just a flash in the pan
GNSS定位精度指标计算
Compilation principle: preprocessing of source program and design and implementation of lexical analysis program (including code)
FairyGUI复选框与进度条的组合使用
FairyGUI循环列表
[offer9] implement queues with two stacks
Mixed use of fairygui button dynamics
第一人称视角的角色移动
最短Hamilton路径 (状压DP)
1041 be unique (20 points (s)) (hash: find the first number that occurs once)
[算法] 劍指offer2 golang 面試題2:二進制加法
地球围绕太阳转
Meanings and differences of PV, UV, IP, VV, CV
Fabrication d'un sac à dos simple fairygui