当前位置:网站首页>[rtklib] preliminary practice of using robust adaptive Kalman filter under RTK
[rtklib] preliminary practice of using robust adaptive Kalman filter under RTK
2022-07-06 12:52:00 【Proletarians】
One 、 background
I used to take measurements when I was in school , Do the static calculation of the coordinates of the station, or control it on the campus rtk Survey topographic points , They are all in a wide field of vision , Exercise slowly . Now use consumer grade receivers , In the dynamic and complex environment of the city , For example, crossing the overpass 、 Under the shade of trees 、 Next to the tall building , There will be a lot of gross errors and cycle slips in the measured values , Very often , It affects the measurement model ; meanwhile , The motion state of the carrier is also unclear , for example , Turn at the intersection , Slow down when encountering pedestrian cars , Speed up after the green light , It affects the dynamic model .
In view of the two aspects of the functional model described above , So in use ekf When , My heart panicked . At this time, consider using robust adaptive Kalman filter , First, robust estimation adjusts the measurement noise matrix , The second is to adaptively adjust the dynamic model , Balance the contributions of both .
Two 、vs Post treatment practice
stay rtklib in , We're doing vs post-processing post When , If Galileo is used 、glonass etc. , Pay attention to defining the macro of the system in the header file , Otherwise, all you get is gps Of rtk result . Since it is dynamic rtk, Then the receiver dynamic Option to open , This can be changed directly in the configuration file . There are two steps to follow , The measurement noise matrix obtained from the determination of adaptive factors and robust estimation .
2.1 Determination of adaptive factor
The adaptive factor can be determined from two aspects , One is the innovation vector , That is, through one-step predicted value and measured value , For the first time ddres Got v; Second, through the status discrepancy , That is, through the state vector at the current time and the modulus of one-step predicted value .
In practical use , It is not recommended to use status discrepancy , Because sometimes the difference in this state is too big , Innovation vector can better reflect the disturbance of dynamic system .
The sample code is as follows :
/* adaptive factor resolved by predicted residual */
static double adaptive_factor(const double *v, int nv)
{
double sum = 0.0,ret;
double c0 = 1.5, c1 = 8.0;
for (int i = 0; i < nv; i++) {
sum += v[i] * v[i];
}
sum = sqrt(sum);
if (sum>c1)
{
ret = 0;
}
else if(sum <= c0) {
ret = 1;
}
else {
ret = (c0 / sum)*SQRT((c1 - sum) / (c1 - c0));
}
return ret;
}
2.2 Tolerance M It is estimated that
Robust estimation is calculated by using a posteriori measurement residual ,rtk Double difference measurements are relevant , Then the equivalent weight strategy cannot be used in robust estimation , Refer to Liu Jingnan's calculation method , And Yang Yuanxi's two factor model is actually the same thing .
The sample code is as follows :
/* Robust estimation algorithm ,RTK Correlation between double difference observations , Equivalent weight scheme cannot be adopted */
/* v I Posterior observation residual vector */
/* R I Measurement noise variance - Covariance matrix */
/* vflag I Double difference satellite innovation field */
/* nv I Measurement noise variance - Covariance matrix dimension namely R The size is (nv*nv) */
static double * robust_estimate(const double *v, const double *R, const int *vflg, int nv)
{ // nv Represents the measurement noise variance - Covariance matrix R Dimension of ,RTK Measurement noise matrix of R It's related. , It is necessary to determine the equivalent variance - Covariance matrix R_, Refer to Liu Jingnan's calculation method
int i, j, sat1, sat2, type, freq;
double *R_;
double w,c=2.0,p; // w Represents the standardized residual ,c~[1.5,3.0],p Indicates the correlation coefficient
R_ = mat(nv, nv);
// step1 First save the diagonal elements
for (i = 0; i < nv; i++) {
w = 0.0;
sat1 = (vflg[i] >> 16) & 0xFF;
sat2 = (vflg[i] >> 8) & 0xFF;
type = (vflg[i] >> 4) & 0xF; // type == 0 ? "L" : (type == 1 ? "P" : "C")
freq = vflg[i] & 0xF;
// Standardized residuals of post test observation residuals
w = fabs(v[i]) / R[i + i*nv];
R_[i + i*nv] = (w < c) ? R[i + i*nv] : w*w*R[i + i*nv];
}
// step2 Then fill in the non diagonal elements , Use the correlation coefficient p Fill in R_
for (i = 0; i < nv; i++) {
for (j = 0; j < nv; j++) {
if (i == j) continue;
p = R[j + i*nv] / (sqrt(R[i + i*nv] * R[j + j*nv]));
R_[j + i*nv] = p*sqrt(R_[i + i*nv] * R_[j + j*nv]);
}
}
return R_;
}
The steps are described in the notes , But this strategy is not right ddpr and ddcp Treat separately , It should be handled separately , because ddcp It may be caused by Zhou Tiao not being eliminated completely , Then the blur will be reset , Just let the code flow out , Let's be a reference .
2.3 Other notes
The adaptive factor is obtained , In Kalman filter filter Function k The calculation of only needs to be made by 1.0 become 1.0/factor that will do .
Of the state vector calculated in the current epoch Pp, I added this
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);
// Zhang Youmin
//matmul("NT", n, n, n, 1.0, Pp, I, 0.0, Pp);
//matmul("NT", m, n, m, 1.0, R, K, 0.0, F_);
//matmul("NN", n, n, m, 1.0, K, F_, 1.0, Pp);
}
result
The result of the test is not good , How to let it out , Ha ha ha . Just to give you a way of thinking ~
边栏推荐
- Database course design: college educational administration management system (including code)
- 燕山大学校园网自动登录问题解决方案
- 染色法判定二分图
- Teach you to release a DeNO module hand in hand
- [Yu Yue education] guide business reference materials of Wuxi Vocational and Technical College of Commerce
- 音乐播放(Toggle && PlayerPrefs)
- KF UD分解之伪代码实现进阶篇【2】
- 堆排序【手写小根堆】
- FairyGUI增益BUFF數值改變的顯示
- FairyGUI简单背包的制作
猜你喜欢
FairyGUI循環列錶
[algorithm] sword finger offer2 golang interview question 4: numbers that appear only once
FairyGUI简单背包的制作
Matlab读取GNSS 观测值o文件代码示例
【干货】提升RTK模糊度固定率的建议之周跳探测
[algorithm] sword finger offer2 golang interview question 13: sum of numbers of two-dimensional submatrix
Prove the time complexity of heap sorting
Fabrication of fairygui simple Backpack
FairyGUI复选框与进度条的组合使用
Unity3D,阿里云服务器,平台配置
随机推荐
Office提示您的许可证不是正版弹框解决
[leetcode15] sum of three numbers
[algorithm] sword finger offer2 golang interview question 4: numbers that appear only once
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
Design and implementation of general interface open platform - (39) simple and crude implementation of API services
Programming homework: educational administration management system (C language)
雇佣收银员【差分约束】
Derivation of logistic regression theory
地球围绕太阳转
dosbox第一次使用
[算法] 剑指offer2 golang 面试题1:整数除法
[Clickhouse kernel principle graphic explanation] about the collaborative work of partitioning, indexing, marking and compressed data
基于rtklib源码进行片上移植的思路分享
[Yu Yue education] guide business reference materials of Wuxi Vocational and Technical College of Commerce
Vulnhub target: hacknos_ PLAYER V1.1
【GNSS数据处理】赫尔默特(helmert)方差分量估计解析及代码实现
[算法] 劍指offer2 golang 面試題2:二進制加法
微信小程序开发心得
FairyGUI增益BUFF数值改变的显示
Mysql database reports an error: row size too large (> 8126) Changing some columns to TEXT or BLOB or using ROW_ FORMAT=DY