当前位置：网站首页>rtklib单点定位spp使用抗差估计遇到的问题及解决
rtklib单点定位spp使用抗差估计遇到的问题及解决
2022-07-06 09:18:00 【Proletarians】
问题描述
在spp中应用抗差估计时，在计算观测值验后残差v的协方差阵的时候，出现了问题，Qvv对角元出现了负值。
首先，我怀疑是自己代码出了问题，紧接着就是调试，在vs中调试矩阵这方面真的是让人头大。
其次，我将spp中的系数矩阵H、常数矩阵l、协方差阵Qxx、参数向量dx打印出来了，在matlab验证对比。
1、matlab中打印上述的矩阵
>> 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、在vs中打印上述矩阵
2.1 打印Q_矩阵
我在vs中计算HQH’时出了问题，代码如下：
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");
其中，matmul是矩阵相乘的函数，T表示转置，N表示不转置；H_是系数矩阵，未加权的系数矩阵，是rescode函数直接返回的H的拷贝，Q是lsq的返回值，Q_表示HQH’，那么按照上述代码得出的打印结果如下图所示：
对角元有负值，并且不是对角矩阵，很明显就是错了嘛，哈哈哈
2.2、打印系数矩阵H
在rescode函数中H是按照[-e1 -e2 -e3 1.0 0.0 0.0 0.0]这样来放在数组或矩阵中的，这样就是nv * NX个元素了，我是按mat定义的NX * nv来打印，再按照nv * NX来打印，结果如下
系数矩阵H说是NX * nv，打印的结果显示内部数据排列应该是上述第二个打印矩阵转置的形式，也就是说H的形式其实就是matlab中H’的形式
2.3、打印H‘ * H
为了验证2.2中上述的结论，就对H’ * H的结果进行打印，如下图所示：
2.4、对2.1中矩阵相乘运算修改验证
先对H和单位矩阵相乘，计算出来H‘，再进行H‘QH运算
// 3、根据协方差传播定律可知，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); /* 求转置矩阵H' */
printmat(tH, NX, nv, "tH");// 打印转置矩阵
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_");
打印结果如下：
其实这个tH就是Matlab中的H的转置形式，再次用调试手段验证了rtklib的矩阵是按第一列、第二列。。。第n列的形式填充矩阵的，跟我们平时自己写代码习惯不一样。打印结果和Matlab一致了。
抗差代码
spp中加入抗差的代码也写完了，测试验证了下，确实加了抗差的rms和std要比不加抗差的小，等我把代码的注释加好，会把 测试数据+代码+结果图 放出来，大家交流一下。
边栏推荐
- （5） Introduction to R language bioinformatics -- ORF and sequence analysis
- MySQL time, time zone, auto fill 0
- VLSM variable length subnet mask partition tips
- JUC forkjoin and completable future
- ESP8266连接onenet（旧版MQTT方式）
- Servlet
- [Clickhouse kernel principle graphic explanation] about the collaborative work of partitioning, indexing, marking and compressed data
- Walk into WPF's drawing Bing Dwen Dwen
- @Autowired 和 @Resource 的区别
- 音乐播放（Toggle ＆＆ PlayerPrefs）
猜你喜欢
数据库课程设计：高校教务管理系统（含代码）
Remember an experience of ECS being blown up by passwords - closing a small black house, changing passwords, and changing ports
Compilation principle: preprocessing of source program and design and implementation of lexical analysis program (including code)
Redis based distributed ID generator
Conditional probability
NovAtel 板卡OEM617D配置步骤记录
Fairygui joystick
Fairygui loop list
（三）R语言的生物信息学入门——Function, data.frame, 简单DNA读取与分析
![[Clickhouse kernel principle graphic explanation] about the collaborative work of partitioning, indexing, marking and compressed data](/img/28/221b0a51ef5f2e8ed5aeca2de8f463.jpg)
[Clickhouse kernel principle graphic explanation] about the collaborative work of partitioning, indexing, marking and compressed data
随机推荐
2022.2.12 resumption
Special palindromes of daily practice of Blue Bridge Cup
[Leetcode15]三数之和
[leetcode19] delete the penultimate node in the linked list
Fairygui gain buff value change display
FairyGUI摇杆
地球围绕太阳转
Liste des boucles de l'interface graphique de défaillance
Mysql database index
dosbox第一次使用
MySQL時間、時區、自動填充0的問題
（5） Introduction to R language bioinformatics -- ORF and sequence analysis
[Nodejs] 20. Koa2 onion ring model ----- code demonstration
341. Flatten nested list iterator
[leetcode19]删除链表中倒数第n个结点
Particle system for introduction to unity3d Foundation (attribute introduction + case production of flame particle system)
[offer9] implement queues with two stacks
音乐播放（Toggle ＆＆ PlayerPrefs）
單片機藍牙無線燒錄
@Autowired 和 @Resource 的区别