1. Background I
used to measure when I was in school, control the coordinates of a static arithmetic station, or measure topographical points on the campus by RTK, all of which were carried out with a wide field of view and slow movement. Nowadays, consumer-grade receivers are used. Under urban dynamic and complex environments, such as overpasses, under trees, and next to tall buildings, the measurement values will have a lot of gross errors and cycle slips, which are very frequent, affecting the measurement model; At the same time, the state of motion of the carrier is also unclear. For example, when turning at an intersection, when encountering a pedestrian, the vehicle has to slow down, and the vehicle has to accelerate after a green light, which affects the dynamic model.
In view of the two aspects of the function model described above, when using ekf, I panicked. At this time, consider the use of robust adaptive Kalman filtering, one is to adjust the measurement noise matrix by robust estimation, and the other is to adaptively adjust the dynamic model to balance the contributions of the two.
Second, vs post-processing practice
In rtklib, when we perform vs post-processing post, if we use Galileo, glonas, etc., we must pay attention to defining system macros in the header file, otherwise we only get the rtk result of gps. Since it is a dynamic rtk, the dynamic option of the receiver should be turned on, which can be changed directly in the configuration file. The following will be carried out in two steps, the determination of the adaptive factor and the measurement noise matrix obtained by robust estimation.
2.1 The determination of the adaptive factor The determination of the
adaptive factor can be from two aspects, one is the innovation vector, which is obtained through one-step predicted value and measured value, which is the v obtained from the first ddres; the other is through the state discrepancy value, That is, through the current state vector and the modulus of the one-step predicted value.
In actual use, it is not recommended to use the state discrepancy value, because this state is sometimes calculated to be too different, and the innovation vector can better reflect the disturbance of the 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 Robust M estimation
Robust estimation is calculated by using post-test measurement residuals. The rtk double difference measurement value is related, so the equivalent weight strategy cannot be used when performing robust estimation. Refer to Liu Jingnan’s The calculation method is actually the same as Yang Yuanxi's two-factor model.
The sample code is as follows:
/* 抗差估计算法，RTK双差观测值之间相关，不能采用等价权方案 */
/* v I 验后观测残差向量 */
/* R I 量测噪声方差-协方差阵 */
/* vflag I 双差卫星新息字段 */
/* nv I 量测噪声方差-协方差阵维数 即R大小为(nv*nv) */
static double * robust_estimate(const double *v, const double *R, const int *vflg, int nv)
{ // nv表示量测噪声方差-协方差阵R的维数，RTK的量测噪声矩阵R是相关的，需要确定等价方差-协方差矩阵R_，参考刘经南计算方法
int i, j, sat1, sat2, type, freq;
double *R_;
double w,c=2.0,p; // w表示标准化残差，c~[1.5,3.0]，p表示相关系数
R_ = mat(nv, nv);
// step1 先保存对角线元素
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;
// 验后观测残差的标准化残差
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 再填充非对角线元素，使用相关系数p来填充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 comments, but in this strategy, ddpr and ddcp are not processed separately, they must be processed separately, because ddcp may be caused by cycle slips that are not cleaned out, so the ambiguity must be reset. Let this code flow be released for reference.
2.3 Other notes The
adaptive factor is obtained. The calculation of k in the Kalman filter function only needs to change from 1.0 to 1.0/factor in the control coefficient.
Pp of the state vector calculated in the current epoch, 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);
// 张友民
//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);
}
The result of the
test is not good, how to put it out, hahaha. Just to provide you with an idea~