当前位置:网站首页>Problems and solutions of robust estimation in rtklib single point location spp
Problems and solutions of robust estimation in rtklib single point location spp
2022-07-06 12:53:00 【Proletarians】
Problem description
stay spp When robust estimation is applied in , After calculating the observed value, the residual error v Covariance matrix of , There is a problem ,Qvv The diagonal element has a negative value .
First , I suspect there is something wrong with my code , The next step is debugging , stay vs The debugging matrix is really impressive .
secondly , I will spp Coefficient matrix in H、 Constant matrix l、 Covariance matrix Qxx、 Parameter vector dx It's printed , stay matlab Verification comparison .
1、matlab Print the above matrix
>> H=[ 0.497179 -0.767231 0.405178 1.000000 0.000000 0.000000 0.000000;
0.954274 0.112689 -0.276879 1.000000 0.000000 0.000000 0.000000;
0.387339 -0.481553 -0.786178 1.000000 0.000000 0.000000 0.000000;
-0.580737 -0.519400 -0.626872 1.000000 0.000000 0.000000 0.000000;
-0.287620 -0.601084 -0.745636 1.000000 0.000000 0.000000 0.000000;
0.394442 -0.847597 -0.354957 1.000000 0.000000 0.000000 0.000000;
0.479621 -0.224119 -0.848371 1.000000 0.000000 0.000000 0.000000;
-0.266211 -0.887121 0.377026 1.000000 0.000000 0.000000 0.000000;
0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000;
0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000;
0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000];
>> Q=[ 1.444742 -1.205538 -0.559907 -1.256723 0.000000 0.000000 0.000000;
-1.205538 4.239855 1.510516 3.308947 0.000000 0.000000 0.000000;
-0.559907 1.510516 1.847995 1.891790 0.000000 0.000000 0.000000;
-1.256723 3.308947 1.891790 3.215663 0.000000 0.000000 0.000000;
0.000000 0.000000 0.000000 0.000000 0.010000 0.000000 0.000000;
0.000000 0.000000 0.000000 0.000000 0.000000 0.010000 0.000000;
0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.010000];
>> dx=[ 0.000009
-0.000020
-0.000022
-0.000041
0.000000
0.000000
0.000000];
>> l=[0.123075
0.433912
0.828791
0.067140
0.026320
-0.289038
-0.862615
-0.198928
0.000000
0.000000
0.000000];
>> H'
ans =
Columns 1 through 8
0.497179000000000 0.954274000000000 0.387339000000000 -0.580737000000000 -0.287620000000000 0.394442000000000 0.479621000000000 -0.266211000000000
-0.767231000000000 0.112689000000000 -0.481553000000000 -0.519400000000000 -0.601084000000000 -0.847597000000000 -0.224119000000000 -0.887121000000000
0.405178000000000 -0.276879000000000 -0.786178000000000 -0.626872000000000 -0.745636000000000 -0.354957000000000 -0.848371000000000 0.377026000000000
1.000000000000000 1.000000000000000 1.000000000000000 1.000000000000000 1.000000000000000 1.000000000000000 1.000000000000000 1.000000000000000
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
Columns 9 through 11
0 0 0
0 0 0
0 0 0
0 0 0
1.000000000000000 0 0
0 1.000000000000000 0
0 0 1.000000000000000
>> H*Q*H'
ans =
Columns 1 through 8
1.332860500714645 0.645546014871716 -0.098580562413659 -0.064031507548390 -0.165752578512155 0.396295832699334 -0.141008818175153 1.168889713304825
0.645546014871717 1.968804844653495 0.053246247795955 -0.005468449432181 -0.265950607478022 -0.319534503408733 0.574091433817809 0.243277031690377
-0.098580562413659 0.053246247795955 0.357273653443425 -0.021840446651663 0.164659427109121 0.283415207166032 0.318871615198842 -0.284958778729781
-0.064031507548390 -0.005468449432181 -0.021840446651663 1.072132904685670 0.641476084869937 -0.226446903091478 0.079281778351138 0.679077103029250
-0.165752578512155 -0.265950607478023 0.164659427109121 0.641476084869937 0.515321629013082 0.067397424711531 0.125308500906703 0.247806596987045
0.396295832699334 -0.319534503408734 0.283415207166032 -0.226446903091478 0.067397424711531 0.647339251042735 0.015423699735895 0.151774558340723
-0.141008818175153 0.574091433817809 0.318871615198843 0.079281778351138 0.125308500906704 0.015423699735896 0.481685375510297 -0.331346855770643
1.168889713304826 0.243277031690377 -0.284958778729780 0.679077103029250 0.247806596987045 0.151774558340723 -0.331346855770643 1.674731658216493
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
Columns 9 through 11
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0
0.010000000000000 0 0
0 0.010000000000000 0
0 0 0.010000000000000
>>
2、 stay vs Print the above matrix
2.1 Print Q_ matrix
I am here vs Middle computation HQH’ There was a problem , The code is as follows :
printf("**********************\n");
Q_ = mat(nv, nv);
matmul("TN", nv, NX, NX, 1.0, H_, Q, 0.0, Q);
matmul("NN", nv, nv, NX, 1.0, Q, H_, 0.0, Q);
printmat(Q, nv, nv, "Q");
among ,matmul Is a function of matrix multiplication ,T Means transpose ,N Means no transpose ;H_ It's the coefficient matrix , Unweighted coefficient matrix , yes rescode Function returns directly H A copy of the ,Q yes lsq The return value of ,Q_ Express HQH’, Then the print result obtained according to the above code is shown in the following figure :
Diagonal elements have negative values , And it's not a diagonal matrix , It's obviously wrong , Ha ha ha
2.2、 Print coefficient matrix H
stay rescode Function H Is in accordance with the [-e1 -e2 -e3 1.0 0.0 0.0 0.0] Put in an array or matrix like this , This is the nv * NX An element , I press mat Defined NX * nv To print , Again according to nv * NX To print , give the result as follows
coefficient matrix H be supposed to NX * nv, The printing result shows that the internal data arrangement should be in the form of transposition of the second printing matrix mentioned above , in other words H The form of is actually matlab in H’ In the form of
2.3、 Print H‘ * H
In order to verify 2.2 The above conclusion , That's right H’ * H Print the results of , As shown in the figure below :
2.4、 Yes 2.1 Modification and verification of matrix multiplication operation in
First pair H Multiply with the identity matrix , Work it out H‘, Proceed again H‘QH operation
// 3、 According to the law of covariance propagation ,Qvv=Qll-B*Q*B',Q=(B'PB)^-1
// lsq: n<m,n=NX m=nv
// matmul("NN",n,1,m,1.0,A,y,0.0,Ay); /* Ay=A*y */
// matmul("NT", n, n, m, 1.0, A, A, 0.0, Q); /* Q=A*A' */
printf("**********************\n");
Q_ = mat(nv, nv);
double *eyeI;
double *tH;
eyeI = mat(NX, NX);
tH = mat(nv, NX);
for (j = 0; j < NX;j++)
{
for (k = 0; k < NX;k++)
{
eyeI[j + k*NX] = (j==k)?1.0:0.0;
}
}
matmul("TN", nv, NX, NX, 1.0, H_, eyeI, 0.0, tH); /* Find the transpose matrix H' */
printmat(tH, NX, nv, "tH");// Print transpose matrix
double *Q1,*Q2;
Q1 = mat(nv, NX);
Q2 = mat(nv, nv);
matmul("NN", nv, NX, NX, 1.0, tH, Q, 0.0, Q1);
matmul("NN", nv, nv, NX, 1.0, Q1, H_, 0.0, Q_);
printmat(Q_, nv, nv, "Q_");
The results are as follows :
Actually this tH Namely Matlab Medium H Transpose form of , It is verified by debugging again rtklib The matrix of is in the first column 、 Second column ... The first n The form of columns fills the matrix , It's different from our usual habit of writing code by ourselves . Print the results and Matlab It's the same .
Robust code
spp The code that adds robustness to the code is also written , The test verifies , It's really anti poor rms and std It's smaller than the one without resistance , Wait for me to add the comments of the code , Will be able to Test data + Code + Result chart release , Let's talk about it .
边栏推荐
- Introduction to the daily practice column of the Blue Bridge Cup
- Particle system for introduction to unity3d Foundation (attribute introduction + case production of flame particle system)
- What is the maximum length of MySQL varchar field
- What are the advantages of using SQL in Excel VBA
- Matlab读取GNSS 观测值o文件代码示例
- [offer9] implement queues with two stacks
- [algorithm] sword finger offer2 golang interview question 8: the shortest subarray with a sum greater than or equal to K
- RTKLIB: demo5 b34f.1 vs b33
- Unity3d camera, the keyboard controls the front and rear left and right up and down movement, and the mouse controls the rotation, zoom in and out
- FairyGUI簡單背包的制作
猜你喜欢
随机推荐
Fairygui character status Popup
【rtklib】在rtk下使用抗差自适应卡尔曼滤波初步实践
In 2020, the average salary of IT industry exceeded 170000, ranking first
[algorithm] sword finger offer2 golang interview question 5: maximum product of word length
MySQL shutdown is slow
Mysql database index
堆排序【手写小根堆】
GNSS定位精度指标计算
Unity3D制作注册登录界面,并实现场景跳转
MySQL performance tuning - dirty page refresh
Fabrication d'un sac à dos simple fairygui
FGUI工程打包发布&导入Unity&将UI显示出来的方式
KF UD分解之UD分解基础篇【1】
[untitled]
FairyGUI增益BUFF數值改變的顯示
[算法] 剑指offer2 golang 面试题10:和为k的子数组
Office提示您的许可证不是正版弹框解决
[offer78]合并多个有序链表
Particle system for introduction to unity3d Foundation (attribute introduction + case production of flame particle system)
[leetcode15] sum of three numbers