[rtklib] preliminary practice of using robust adaptive Kalman filter under RTK

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 ～