rtklib单点定位

清风忘忧

已于 2022-10-30 00:45:36 修改

阅读量1k

点赞数 1

分类专栏： GNSS 文章标签： ssl

于 2022-10-30 00:43:34 首次发布

本文链接：https://blog.csdn.net/u014430081/article/details/127592962

版权

GNSS 专栏收录该内容

10 篇文章 11 订阅

订阅专栏

前言

单点定位是卫星定位里面最基本的一个定位算法，基本原理是：假如在地上的一个点A，能观测到4颗卫星（卫星位置已知），通过后方交汇原理就可以求出点A的概略位置，精度在m级。

单点定位算法除了能给提供一个概略位置外，还有其他的用途：例如，
1、在rtcm格式数据解码的时候，我们得外部传入一个概略时间才能对观测数据赋值准确的时间，一般我们是先解码出星历数据，再将星历的时间当作概略时间传入，但我们怎么确定这个概略时间是对的呢，这时我们求一个单点解，如果能通过单点定位解算，就可以确定这个时间是对的；
2、在做gnss板卡解码的时候，你可以用解码的数据进行单点定位解算，如果能成功，那就说明解码数据没问题了；
3、单点定位的时候，我们会求出GDOP/PDOP等精度因子指标，根据这个我们可以粗略判断当前的定位环境，跟其他传感器组合的时候，根据这些值调节各传感器数据的权重。

一、代码

基于rtklib_demo5中单点定位代码进行的精度提升试验，用2种方法剔除一下数据：第一种根据残差和先验方差来选卫星；第二种按残差大小选卫星；
结果如下：
在这里插入图片描述
结果：2种方法都可以剔除部分差的数据，定位精度都有提升；方法1比方法2效果更好，结果更稳健一点。

代码如下：


按绝对值从小到大排序，返回原先对应的下标
type 1;从小到大
///type 2 ;从大到小
static void indexsort(double * data, int length, int* index,int type)
{
	double exchange = 0;
	int t1 = 0;
	for (int i = 1; i <= length - 1; i++)
	{
		for (int j = 1; j <= length - i; j++)
		{
			if (1 == type)
			{
				if (fabs(data[j - 1]) >= fabs(data[j]))
				{
					exchange = data[j - 1];
					data[j - 1] = data[j];
					data[j] = exchange;

					t1 = index[j - 1];
					index[j - 1] = index[j];
					index[j] = t1;
				}
			}
			if (2 == type)
			{
				if (fabs(data[j - 1]) <= fabs(data[j]))
				{
					exchange = data[j - 1];
					data[j - 1] = data[j];
					data[j] = exchange;

					t1 = index[j - 1];
					index[j - 1] = index[j];
					index[j] = t1;
				}
			}
	}
	}	
}

#define MAX_ESTPOS 12

/* estimate receiver position ------------------------------------------------*/
static int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
	const double *vare, const int *svh, const nav_t *nav,
	const prcopt_t *opt, const ssat_t *ssat, sol_t *sol, double *azel,
	int *vsat, double *resp, char *msg)
{
	double x[NX] = { 0 }, dx[NX], Q[NX*NX], *v, *H, *var, sig, *v_, *el_;
	int *v_index, *el_index;
	int i, j, k, info, stat, nv, ns;
	int ref = 0;
	int k0 = 50, k1 = 5;
	trace(3, "estpos  : n=%d\n", n);
	int robust_flag = 0;
	double sum_v = 0;
	double epoch[6] = { 0 };
	int validenum = 0;
	v = mat(n + 4, 1); H = mat(NX, n + 4); var = mat(n + 4, 1);
	v_ = mat(n + 4, 1), v_index = imat(n + 4, 1), el_index = imat(n + 4, 1);
	el_ = mat(n + 4, 1);
	int start_index = 0;

	for (i = 0; i<3; i++) x[i] = sol->rr[i];

	for (i = 0; i<MAXITR; i++)
	{
		/* pseudorange residuals (m) */
		sum_v = 0;
		robust_flag = 0;
		validenum = 0;

		nv = rescode(i, obs, n, rs, dts, vare, svh, nav, x, opt, ssat, v, H, var, azel, vsat, resp,
			&ns);
		if (nv<NX)
		{
			sprintf(msg, "lack of valid sats ns=%d", nv);
			break;
		}

	
		//----------------先判断一下残差大小，是否迭代收敛了----------------
		for (j = 0; j < ns; j++)
		{
			sum_v += fabs(v[j]);
		}
		sum_v = sum_v / (ns + 0.001);
		if (sum_v < 10)   
		{
			robust_flag = 1;
		}
		validenum = nv;

		//----------------开始数据筛选----------------
		if (robust_flag == 1) // robust estimate start
		{

#if 1
			//第一步；剔除残差大的数据
			for (j = 0; j < nv; j++)
			{
				if (fabs(v[j] / sqrt(var[j])) > k1   &&  fabs(v[j])>0.001)
				{
					if (validenum > NX)
					{
						v[j] = 0;
						for (k = 0; k < NX; k++) H[k + j*NX] = 0;
						validenum--;
					}
				}
			}

#endif

#if 0
			/第二步，选取残差最小的MAX_ESTPOS颗卫星进行单点定位
			if (validenum > MAX_ESTPOS)
			{
				validenum = MAX_ESTPOS;
				int start_index = 0;
				matcpy(v_, v, nv, 1);

				for (j = 0; j < nv; j++) v_index[j] = j;
				indexsort(v_, nv, v_index, 1);

				for (j = 0; j < nv; j++)
				{
					trace(1, " v_time=%s  index=%d    v=%6.3f   \n", time_s, v_index[j], v_[j]);
				}

				for (j = 0; j < nv; j++)
				{
					if (fabs(v_[j]) > 0.001)
					{
						start_index = j;
						break;
					}
				}

				for (j = start_index; j < nv; j++)
				{

					if (j < nv && j < start_index + MAX_ESTPOS) continue;
					else
					{
						v[v_index[j]] = 0;
						for (k = 0; k < NX; k++) H[k + v_index[j] * NX] = 0;
					}
				}

			}
#endif 

		}


		/* weighted by Std */
		for (j = 0; j<nv; j++)
		{
			sig = sqrt(var[j]);
			v[j] /= sig;
			for (k = 0; k<NX; k++) H[k + j*NX] /= sig;
		}
		/* least square estimation */
		if ((info = lsq(H, v, NX, nv, dx, Q)))
		{
			sprintf(msg, "lsq error info=%d", info);
			break;
		}
		for (j = 0; j<NX; j++)
		{
			x[j] += dx[j];
		}

		if (norm(dx, NX)<1E-4)
		{
			sol->type = 0;
			sol->time = timeadd(obs[0].time, -x[3] / CLIGHT);
			sol->dtr[0] = x[3] / CLIGHT; /* receiver clock bias (s) */
			sol->dtr[1] = x[4] / CLIGHT; /* GLO-GPS time offset (s) */
			sol->dtr[2] = x[5] / CLIGHT; /* GAL-GPS time offset (s) */
			sol->dtr[3] = x[6] / CLIGHT; /* BDS-GPS time offset (s) */
			sol->dtr[4] = x[7] / CLIGHT; /* IRN-GPS time offset (s) */
			for (j = 0; j<6; j++) sol->rr[j] = j<3 ? x[j] : 0.0;
			for (j = 0; j<3; j++) sol->qr[j] = (float)Q[j + j*NX];
			sol->qr[3] = (float)Q[1];    /* cov xy */
			sol->qr[4] = (float)Q[2 + NX]; /* cov yz */
			sol->qr[5] = (float)Q[2];    /* cov zx */
			sol->ns = (uint8_t)ns;
			sol->age = sol->ratio = 0.0;

			/* validate solution */
			if ((stat = valsol(azel, vsat, n, opt, v, nv, NX, msg))) {
				sol->stat = opt->sateph == EPHOPT_SBAS ? SOLQ_SBAS : SOLQ_SINGLE;
			}
			free(v); free(H); free(var);
			return stat;
		}
	}
	if (i >= MAXITR) sprintf(msg, "iteration divergent i=%d", i);

	free(v); free(H); free(var);
	free(v_); free(v_index);
	return 0;
}