Velocity in rtklib
In rtklib, the PVA model in the RTK algorithm does not use doppler observations to update the velocity. The velocity in the dynamic model in rtklib is actually the result of position epoch difference, and the acceleration is the result of velocity difference. When the state is updated, the predicted state is obtained by multiplying the state of the previous epoch by the state transition matrix, then there will be a correlation between position/velocity/acceleration in the covariance matrix of the predicted state.
When using the observed value to update the position, due to the correlation between each state quantity, the velocity and acceleration will change accordingly. But in essence, there is only the update information of the position state quantity, so the accuracy of the velocity and acceleration will be relatively low, and the motion state model of the carrier cannot be better utilized.
The most obvious feature of doppler observations relative to pseudoranges is that they are basically not affected by the ionosphere and troposphere. In addition, the accuracy of the velocity term of the satellite's orbit and clock error is relatively high. Therefore, in theory, the accuracy of carrier velocity measurement using doppler can generally reach the order of centimeters to decimeters.
Update with doppler
In order to make the state prediction accuracy of single-point positioning higher, I plan to increase the use of doppler observations in single-point positioning.
Considering that the current kalman filter single-point positioning state quantity has been determined, that is, position/velocity/acceleration/receiver clock of each system, the receiver clock drift is not considered in the state quantity.
At the same time, considering that in the future RTK algorithm, there is no state quantity related to the receiver clock, so this time we take the inter-satellite single difference for the doppler observation value to eliminate the receiver clock drift error, which is more in line with our current kalman single point positioning process. It will also be easier to reuse when expanding the RTK algorithm in the future.
In the original LSQ single-point positioning process, there are related codes in RTKLIB that use least squares to calculate the receiver speed based on doppler. This time it only needs to be slightly modified to increase the adaptation to multi-frequency. At the same time, I will add comments to the subsequent code to make it easier for beginners to understand. The doppler residual calculation function is as follows:
/* range rate residuals of mul-freq
*
* Input:
* @obs: observations
* @n: number of observations
* @rs: satelltes' postion and velocity
* @dts: satellites' clock and clock drift
* @nav: ephemeris
* @rr: receiver approximate positon
* @x: velocity state in filter
* @azel: azimuth and elevation of satellites
* @vsat: satellites is visiable?
*
* Output:
* @v: doppler residuals
* @var: variance
* @H: design matrix
*
*/
static int resdop_mulfreq_filter(const obsd_t *obs, const int n, const double *rs, const double *dts,
const nav_t *nav, const double *rr, const prcopt_t *opt, const double *x,
const double *azel, const int *vsat, double *v, double *var, double *H)
{
double freq, rate, pos[3], E[9], a[3], e[3], vs[3], cosel, sig, e_ref[3] = {0.0}, v_ref = 0.0;
int i, j, nv = 0, sys, freq_idx;
trace(3, "resdop : n=%d\n", n);
ecef2pos(rr, pos);
xyz2enu(pos, E);
for (i = 0; i < n && i < MAXOBS; i++)
{
if (!(sys = satsys(obs[i].sat, NULL)) || !vsat[i] || norm(rs + 3 + i * 6, 3) <= 0.0)
{
continue;
}
/* LOS (line-of-sight) vector in ECEF */
cosel = cos(azel[1 + i * 2]);
a[0] = sin(azel[i * 2]) * cosel;
a[1] = cos(azel[i * 2]) * cosel;
a[2] = sin(azel[1 + i * 2]);
matmul("TN", 3, 1, 3, 1.0, E, a, 0.0, e);
/* satellite velocity relative to receiver in ECEF */
for (j = 0; j < 3; j++)
{
vs[j] = rs[j + 3 + i * 6] - x[j];
}
/* range rate with earth rotation correction */
rate = dot(vs, e, 3) + OMGE / CLIGHT * (rs[4 + i * 6] * rr[0] + rs[1 + i * 6] * x[0] - rs[3 + i * 6] * rr[1] - rs[i * 6] * x[1]);
for (freq_idx = 0; freq_idx < opt->nf; freq_idx++)
{
freq = sat2freq(obs[i].sat, obs[i].code[freq_idx], nav);
if (obs[i].D[freq_idx] == 0.0 || freq == 0.0)
{
continue;
}
/* range rate residual (m/s). */
v[nv] = (-obs[i].D[freq_idx] * CLIGHT / freq - (rate - CLIGHT * dts[1 + i * 2])) - v_ref;
/* variance of pseudorange error */
var[nv] = varerr(opt, azel[1 + i * 2], sys);
/* design matrix */
for (j = 0; j < 3; j++)
{
H[j + 3 + nv * NX_F] = -e[j] - e_ref[j];
}
if (v_ref == 0.0)
{
v_ref = v[nv];
for (j = 0; j < 3; j++)
{
e_ref[j] = -e[j];
}
continue;
}
if (fabs(H[3 + nv * NX_F]) < 1e-8)
{
continue;
}
nv++;
}
}
return nv;
}In addition, the logic of calling this function is relatively simple, pay attention to the compatibility with the pseudorange residual. For details, please see the code submission record.
nv += resdop_mulfreq_filter(obs, n, rs, dts, nav, x, &rtk->opt, x + 3, azel, vsat, v + nv, var + nv, H + nv * NX_F);Please reply to git to obtain the code in the background of the official account, and then remember to switch to the mulfreq-spp branch.
The commit submitted by this revision:
4e0fb34e8b0d5e91ba638fddcc899f7d9e191b58
Result analysis
At the same time, a set of dynamic data is also submitted. The dynamic data comes from
（https://github.com/IPNL-POLYU/UrbanNavDataset）
I also forked it to the domestic gitee
（https://gitee.com/wutong2008/UrbanNavDataset2 ）
At the same time, in order to compare your own results, the error with the provided benchmark results. I wrote a simple comparison tool using python, and submitted it to the code base with the previous commit. When I have time, I will briefly introduce how to use it. The logic is very simple, and you can use it yourself.
The original RTKLIB single-point positioning results, that is, using the pseudo-range observation value of the first frequency point, the ionosphere and troposphere are both corrected by the model, and the least squares is used for data processing.
As a result there is more epoch loss.
Further investigation found that the reason for the loss was that a large number of epochs could not pass the residual chi-square test in the valpos function. After disabling the valpos function, the results were as follows, which was significantly improved. Because it is a city dynamic result, and the ublox consumer-grade module is used, the result is poor within expectations.
Using the results of the kalman filter I wrote, using the first frequency point pseudo-range observation value, the ionosphere and the troposphere are both corrected by the model, and the data is processed using the kalman filter. From the results, the kalman filter has no significant improvement compared with the least squares, because the accuracy of our predicted state quantities is too low.
Kalman filtering result, configuration unchanged, increase doppler observation value, update speed. As a result, compared with before adding doppler, the "glitch points" were significantly reduced, and the positioning accuracy was also improved to a certain extent.
But the positioning accuracy is still poor, because there are gross errors in the observations, that is, the variance information of some observations is inappropriate. Further optimization is required.
No public
Sometimes codes or resources are placed on the personal official account. If you have any questions, you can reply in the background of the official account, and the answer is faster. Welcome to pay attention to GNSS and automatic driving
Summary of links to other related articles
GNSS Algorithm Learning Series Tutorial - Article List_Indus Fighting's Blog-CSDN Blog