GNSS网平差之间接平差(C语言)

GNSS网间接平差(等精度观测,P=1)

  1. 摘要
    误差理论于测量平差基础。对GPS网测得的基线数据进行间接平差。工具:C语言。

  2. .实习流程
    (1)读取数据文件
    (2)间接平差理论基础:
    观测值的平差值=计算出的观测值初值+改正值
    观测值初值=观测值计算出的各个点坐标之差
    X^i是平差值,Xi0是初值
    在这里插入图片描述
    列出误差方程:
    在这里插入图片描述
    其中红色画圈的记为 “l”,绿色系数为B
    则误差方程为:
    在这里插入图片描述
    计算出V后与观测值求和,得到最终的平差值。
    假设有m条基线,n个点,每条基线有3个观测值,给定一个已知点:则
    V矩阵的形式:3m行,1列
    B矩阵的形式:3
    m行,3*(n-1)列
    L矩阵的形式:3*(n-1)行,1列

  3. 代码实现C语言

#include"stdafx.h"
#include <iostream>
#include<stdio.h>
#include<stdlib.h>
#include<Windows.h>
using namespace std;
typedef struct {
	int seq;
	int begin;
	int end;
	double  dx = 0, dy = 0, dz = 0;
}BaseLine;//基线的结构体,存储基线的起点,终点序号,和基线序号,以及基线的观测值dx,dy,dz
typedef struct {
	double  x = 0;
	double  y = 0, z = 0;
	bool flag = 0;
}Pt;//点的结构体,存储每个点的坐标,flag用来判断此点是否已经计算过初值
typedef struct {
	double  x = 0;
	double  y = 0, z = 0;
	bool flag = 0;
}W;//误差项,存储l矩阵的
#define N 39
void getMatrix(BaseLine *line);//用来读取基线文件
void getInitial_Value(BaseLine *line, Pt *pt);//计算各个点的初值
float getL(Pt *pt, BaseLine *line);//计算L矩阵
void TransMatrix(int Mat[24 * 3][3 * 13], int newMat[3 * 13][3 * 24]);//矩阵转置
void Gauss(int A[][N], float B[][N], int n);//矩阵求逆

int main()
{
	
	Pt  pt[14];
	BaseLine line[24];
	//读取基线观测值文件,注意,文件中的顶点P1,P2什么的都改成1,2,3...,不然运行就会Error。
	getMatrix(line);

	//printf("序号 |    起点 |    终点  | △X | △Y  | △Z \n");
	/*for (int i=0;i<24;i++)
	printf("%d   |    P%d  |    P%d   | %f  | %f   |  %f\n", line[i].seq, line[i].begin, line[i].end, line[i].dx, line[i].dy, line[i].dz);*/

	//以P4为起点,计算其他点坐标初值
	getInitial_Value(line, pt);
	//输出点坐标
	/*for (int j = 0; j < 14; j++)
	printf("%f  %f  %f\n", pt[j].x, pt[j].y, pt[j].z);*/
	/*int i = 0, j = 0;
	int arr1[2][2] = { { 10,20 },{ 30,40 } };
	for (i = 0; i < 2; i++)
	{
	for (j = 0; j < 2; j++)
	{
	arr1[i][j] = i + j;
	printf("%3d", arr1[i][j]);
	}
	printf("\n");
	}*/

	//计算L 矩阵****************************************************************
	float l[24 * 3];
	W L[24];
	for (int i = 0; i < 24; i++)
	{
		L[i].x = line[i].dx - (pt[line[i].end - 1].x - pt[line[i].begin - 1].x);
		L[i].y = line[i].dy - (pt[line[i].end - 1].y - pt[line[i].begin - 1].y);
		L[i].z = line[i].dz - (pt[line[i].end - 1].z - pt[line[i].begin - 1].z);
	}
	//for (int i = 0; i < 24; i++)
	//{//输出L矩阵
	//printf("%f |  %f |  %f\n", L[i].x, L[i].y, L[i].z);
	//}
	int i = 0;
	for (int j = 0; i<24; j += 3, i++)
	{
		l[j] = L[i].x;
		l[j + 1] = L[i].y;
		l[j + 2] = L[i].z;

	}
	//输出l矩阵
	/*for (int i = 0; i < 72; i++)
	{
	printf("%f\n", l[i]);
	}*/
	/*********************************************************************/
	//计算B矩阵
	int t = 0;
	int B[3 * 24][3 * 13] = { 0 };
	for (int i = 0; i <24 * 3;)
	{
		if ((line[t].end == 1))
		{
			B[i][(line[t].begin - 1) * 3 - 3] = -1;
			B[i + 1][(line[t].begin - 1) * 3 - 2] = -1;
			B[i + 2][(line[t].begin - 1) * 3 - 1] = -1;

		}
		else if ((line[t].begin == 1))
		{
			B[i][(line[t].end - 1) * 3 - 3] = 1;
			B[i + 1][(line[t].end - 1) * 3 - 2] = 1;
			B[i + 2][(line[t].end - 1) * 3 - 1] = 1;
		}
		else
		{
			B[i][(line[t].begin - 1) * 3 - 3] = -1;
			B[i + 1][(line[t].begin - 1) * 3 - 2] = -1;
			B[i + 2][(line[t].begin - 1) * 3 - 1] = -1;
			B[i][(line[t].end - 1) * 3 - 3] = 1;
			B[i + 1][(line[t].end - 1) * 3 - 2] = 1;
			B[i + 2][(line[t].end - 1) * 3 - 1] = 1;
		}
		t++;
		i += 3;


	}
	//输出B矩阵
	//for (int i = 0; i < 3 * 24; i++)
	//	for (int j = 0; j < 3 * 13; j++)
	//	{
	//		
	//		if (j == 3 * 13 - 1)
	//		{
	//			printf("%d", B[i][j]);
	//			printf("\n");
	//		}
	//		else
	//		{
	//			if (j == 0&&(i%3==0||i==0))
	//				printf("Line%d:   ", i/3 + 1);
	//			printf("%d ,", B[i][j]);
	//		}
	//	}
	求B的转置矩阵BT
	int BT[39][72];
	TransMatrix(B, BT);
	/*for (int i = 0; i < 13*3; i++)
	{
	for (int j = 0; j < 3*24; j++)
	{
	printf("%d,", BT[i][j]);
	if (j == 3 * 24 - 1)
	printf("\n");
	}
	}*/
	int NBB[3 * 13][3 * 13] = { 0 };
	for (int i = 0; i < 13 * 3; i++)
	{
		for (int j = 0; j < 13 * 3; j++)
		{
			int sum = 0;
			for (int k = 0; k < 24 * 3; k++)
			{
				sum += BT[i][k] * B[k][j];
			}
			NBB[i][j] = sum;
		}
	}
	/*for (int i = 0; i < 39; i++)
	{
		
		for (int j = 0; j < 39; j++)
		{
			
			if (j == 38)
			{
				printf("%d", NBB[i][j]);
				printf("\n");
			}
			else
				printf("%d,", NBB[i][j]);
		}
	}*/
	//求NBB的逆           公式:x=inv(NBB)*B'*l
   float invNBB[39][39];
   Gauss(NBB, invNBB, 39);
  /* for (int i = 0; i<39; i++)
   {//输出invNBB
	   for (int j = 0; j<39; j++)
	   {
		   printf("%.3lf ", (double)invNBB[i][j] );
	   }
	   printf("\n");
   }*/

   //求x,待解参数的误差
   float temp1[3 * 13][3 * 24] = { 0 };
   for (int i = 0; i < 13 * 3; i++)
   {
	   for (int j = 0; j < 24 * 3; j++)
	   {
		   float sum = 0;
		   for (int k = 0; k < 13 * 3; k++)
		   {
			   sum += invNBB[i][k] * BT[k][j];
		   }
		   temp1[i][j] = sum;
	   }
   }
  /* for (int i = 0; i < 39; i++)
   {

       for (int j = 0; j < 72; j++)
      {
  
             if (j == 71)
            {
			   printf("%f", temp1[i][j]);
                printf("\n");
             }
             else
                printf("%f,", temp1[i][j]);
       }
   }*/
   //解得待解参数x改正值,39*1
   float x[39] = { 0 };
   for (int i = 0; i < 13 * 3; i++)
   {
	   float sum = 0;
	   for (int k = 0; k < 24 * 3; k++)
	   {
		   sum += temp1[i][k] * l[k];
	   }
	   x[i] = sum;
   }
   float V[72];
   for (int i = 0; i < 24 * 3; i++)
   {
	   float sum = 0;
	   for (int k = 0; k < 13 * 3; k++)
	   {
		   sum += B[i][k] * x[k]-l[k];
	   }
	   V[i] = sum;
   }
   //输出每个观测值的改正值:
   printf("每个观测值的改正值:\n");
   const char *pFileName = "间接平差result.txt";
   FILE * pFile;
   pFile = fopen(pFileName, "w");
   if (NULL == pFile)
   {
	   printf("error");
	   return 0;
   }
    for (int i = 0; i < 72; i++)
    {
		printf("%d :   %.4f  \n",i+1,V[i]);
		fprintf(pFile, "V[%d] :   %.4f  \n", i + 1, V[i]);
    }
	fclose(pFile);
	int vv = 0;
	for (int i = 0; i < 72; i += 3)
	{
		line[vv].dx = line[vv].dx + V[i];
		line[vv].dy = line[vv].dy + V[i+1];
		line[vv].dz = line[vv].dz + V[i+2];
		vv++;
	}
	//输出最终平差值
	printf("最终平差结果为:\n");
	pFile = fopen(pFileName, "a");
	if (NULL == pFile)
	{
		printf("error ");
		return 0;
	}	for (int i = 0; i < 24; i++)
	{
		printf("%.3f  |   %.3f    |   %.3f    \n", line[i].dx, line[i].dy, line[i].dz);
		fprintf(pFile, "%.3f  |    %.3f    |    %.3f\n", line[i].dx, line[i].dy, line[i].dz);

	}
	fclose(pFile);
	printf("名为:‘间接平差result.txt’的文件已经存入本目录\n");
   system("pause");
	return 0;
	
}
void getMatrix(BaseLine *line)
{
	FILE* fp;
	fp = fopen("GNSS.txt", "r");//读取文件,改成自己的路径
	if (!fp)
	{//报错
		printf("ERROR!");
		exit(0);
	}
	int t = 0;
	int i = 0;
	float f = 0;
	while (!feof(fp))
	{
		fscanf_s(fp, "%d,", &t);
		line[i].seq = t;
		fscanf_s(fp, "%d,", &t);
		line[i].begin = t;
		fscanf_s(fp, "%d,", &t);
		line[i].end = t;
		fscanf_s(fp, "%f,", &f);
		line[i].dx = f;
		fscanf_s(fp, "%f,", &f);
		line[i].dy = f;
		fscanf_s(fp, "%f\n", &f);
		line[i].dz = f;
		i++;
	}
	fclose(fp);//关闭文件
}
void getInitial_Value(BaseLine *line, Pt *pt)
{
	float X0 = 0, Y0 = 0, Z0 = 0;
	printf("请输入P1的(X,Y,Z)坐标,作为起算点\n");
	scanf_s("%f ,%f, %f", &X0, &Y0, &Z0);
	pt[0].x = X0;
	pt[0].y = Y0;
	pt[0].z = Z0;
	pt[0].flag = 1;
	//计算其他点坐标初值x0,y0,z0;
	for (int j = 0; j < 24;)
	{
		if ((pt[line[j].end - 1].flag == 0) && (pt[line[j].begin - 1].flag == 1))
		{//终点没有算过初值,起点有初值
			pt[line[j].end - 1].x = pt[line[j].begin - 1].x + line[j].dx;
			pt[line[j].end - 1].y = pt[line[j].begin - 1].y + line[j].dy;
			pt[line[j].end - 1].z = pt[line[j].begin - 1].z + line[j].dz;
			pt[line[j].end - 1].flag = 1;
			j++;

		}
		else if ((pt[line[j].end - 1].flag == 1) && (pt[line[j].begin - 1].flag == 0))
		{//终点算过初值,起点没初值
			pt[line[j].begin - 1].x = pt[line[j].end - 1].x - line[j].dx;
			pt[line[j].begin - 1].y = pt[line[j].end - 1].y - line[j].dy;
			pt[line[j].begin - 1].z = pt[line[j].end - 1].z - line[j].dz;
			pt[line[j].begin - 1].flag = 1;
			j++;

		}
		else if ((pt[line[j].end - 1].flag == 1) && (pt[line[j].begin - 1].flag == 1))
		{//都算过了
			j++;

		}

	}
}
void TransMatrix(int Mat[24 * 3][3 * 13], int newMat[3 * 13][3 * 24])
{//B矩阵转置
	for (int i = 0; i < 72; i++)
	{
		for (int j = 0; j < 3 * 13; j++)
		{
			newMat[j][i] = Mat[i][j];
		}
	}

	//for (int i = 0; i < 3 * 24; i++)
	//	for (int j = 0; j < 3 * 13; j++)
	//	{
	//		
	//		if (j == 3 * 13 - 1)
	//		{
	//			printf("%d", Mat[i][j]);
	//			printf("\n");
	//		}
	//		else
	//		{
	//			/*if (j == 0&&(i%3==0||i==0))
	//				printf("Line%d:   ", i/3 + 1);*/
	//			printf("%d ,",Mat[i][j]);
	//		}
	//	}
}


//高斯求逆
void Gauss(int A[][N], float B[][N], int n)
{
	int i, j, k;
	float max, temp;
	float t[N][N];                //临时矩阵
								  //将A矩阵存放在临时矩阵t[n][n]中
	for (i = 0; i < n; i++)
	{
		for (j = 0; j < n; j++)
		{
			t[i][j] = A[i][j];
		}
	}
	//初始化B矩阵为单位阵
	for (i = 0; i < n; i++)
	{
		for (j = 0; j < n; j++)
		{
			B[i][j] = (i == j) ? (float)1 : 0;
		}
	}
	for (i = 0; i < n; i++)
	{
		//寻找主元
		max = t[i][i];
		k = i;
		for (j = i + 1; j < n; j++)
		{
			if (fabs(t[j][i]) > fabs(max))
			{
				max = t[j][i];
				k = j;
			}
		}
		//如果主元所在行不是第i行,进行行交换
		if (k != i)
		{
			for (j = 0; j < n; j++)
			{
				temp = t[i][j];
				t[i][j] = t[k][j];
				t[k][j] = temp;
				//B伴随交换
				temp = B[i][j];
				B[i][j] = B[k][j];
				B[k][j] = temp;
			}
		}
		//判断主元是否为0, 若是, 则矩阵A不是满秩矩阵,不存在逆矩阵
		if (t[i][i] == 0)
		{
			printf("There is no inverse matrix!");
			system("pause");
			exit(0);
		}
		//消去A的第i列除去i行以外的各行元素
		temp = t[i][i];
		for (j = 0; j < n; j++)
		{
			t[i][j] = t[i][j] / temp;        //主对角线上的元素变为1
			B[i][j] = B[i][j] / temp;        //伴随计算
		}
		for (j = 0; j < n; j++)        //第0行->第n行
		{
			if (j != i)                //不是第i行
			{
				temp = t[j][i];
				for (k = 0; k < n; k++)        //第j行元素 - i行元素*j列i行元素
				{
					t[j][k] = t[j][k] - t[i][k] * temp;
					B[j][k] = B[j][k] - B[i][k] * temp;
				}
			}
		}
	}
}

4.输入文件与输出结果结果
输入文件格式:
这里把所有的顶点的字母"P"去掉了
在这里插入图片描述
结果:
在这里插入图片描述
C语言的一些矩阵运算

  • 9
    点赞
  • 54
    收藏
    觉得还不错? 一键收藏
  • 20
    评论
以下是一个简单的GNSS自由网平差C++程序的编写步骤: 1. 定义数据结构:定义一个结构体来存储观测值、坐标等数据。例如: ``` struct Observation { double x; //观测点 X 坐标 double y; //观测点 Y 坐标 double z; //观测点 Z 坐标 double rho; //伪距观测值 double sigma; //观测值的误差方差 }; ``` 2. 读取数据:从文件中读取观测值、坐标等数据,并存储到定义好的数据结构中。 3. 构建设计矩阵:根据自由网平差的原理,构建设计矩阵。设计矩阵的每一行对应一个观测方程,每一列对应一个未知数。例如: ``` //构建设计矩阵 Eigen::MatrixXd A(num_observations, num_unknowns); for (int i = 0; i < num_observations; i++) { A(i, 0) = (ref_x - observations[i].x) / observations[i].rho; A(i, 1) = (ref_y - observations[i].y) / observations[i].rho; A(i, 2) = (ref_z - observations[i].z) / observations[i].rho; A(i, 3) = 1.0; } ``` 其中,ref_x、ref_y、ref_z是参考点的坐标,num_observations是观测方程的个数,num_unknowns是未知数的个数。 4. 构建观测向量:将观测值存储到一个向量中。例如: ``` //构建观测向量 Eigen::VectorXd L(num_observations); for (int i = 0; i < num_observations; i++) { L(i) = observations[i].rho - sqrt(pow(ref_x - observations[i].x, 2) + pow(ref_y - observations[i].y, 2) + pow(ref_z - observations[i].z, 2)); } ``` 5. 构建权矩阵:根据观测值的误差方差,构建权矩阵。例如: ``` //构建权矩阵 Eigen::MatrixXd P(num_observations, num_observations); for (int i = 0; i < num_observations; i++) { P(i, i) = 1.0 / observations[i].sigma; } ``` 6. 求解未知数:使用最小二乘法,求解未知数。例如: ``` //求解未知数 Eigen::VectorXd X = (A.transpose() * P * A).inverse() * A.transpose() * P * L; ``` 其中,X是求解得到的未知数向量。 7. 输出结果:将求解得到的未知数输出到文件中。 以上是一个简单的GNSS自由网平差C++程序的编写步骤,具体实现可以根据实际需求进行修改和完善。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 20
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值