GNSS网平差之间接平差（C语言）

一向

已于 2022-04-04 14:28:43 修改

阅读量3.7k

点赞数 9

文章标签：平差误差 GNSS

于 2021-05-09 17:57:56 首次发布

本文链接：https://blog.csdn.net/qq_46877697/article/details/116567439

版权

本文介绍使用C语言实现GNSS网间接平差的过程。针对等精度观测数据，通过读取基线文件、计算各点坐标初值、构建并求解误差方程，最终输出平差结果。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

GNSS网间接平差（等精度观测，P=1）

摘要
误差理论于测量平差基础。对GPS网测得的基线数据进行间接平差。工具：C语言。
.实习流程
（1）读取数据文件
（2）间接平差理论基础：
观测值的平差值=计算出的观测值初值+改正值
观测值初值=观测值计算出的各个点坐标之差
X^i是平差值，Xi0是初值

列出误差方程：

其中红色画圈的记为 “l”，绿色系数为B
则误差方程为：

计算出V后与观测值求和，得到最终的平差值。
假设有m条基线,n个点，每条基线有3个观测值，给定一个已知点：则
V矩阵的形式：3m行，1列
B矩阵的形式：3m行，3*（n-1）列
L矩阵的形式：3*（n-1）行，1列
代码实现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;
				}
			}
		}
	}
}