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

﹏ヽ暗。殇╰゛Y 2023-01-19 09:44 119阅读 0赞

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

1.  **摘要**  
    误差理论于测量平差基础。对GPS网测得的基线数据进行间接平差。工具：C语言。
2.  .**实习流程**  
    （1）读取数据文件  
    （2）间接平差理论基础：  
    观测值的平差值=计算出的观测值初值+改正值  
    观测值初值=观测值计算出的各个点坐标之差  
    X^i是平差值，Xi0是初值  
    ![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQ2ODc3Njk3_size_16_color_FFFFFF_t_70]  
    列出误差方程：  
    ![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQ2ODc3Njk3_size_16_color_FFFFFF_t_70 1]  
    其中红色画圈的记为 “l”，绿色系数为B  
    则误差方程为：  
    ![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQ2ODc3Njk3_size_16_color_FFFFFF_t_70 2]  
    计算出V后与观测值求和，得到最终的平差值。  
    假设有m条基线,n个点，每条基线有3个观测值，给定一个已知点：则  
    V矩阵的形式：3*m行，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];
    	//读取基线观测值文件
    	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.**结果**  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQ2ODc3Njk3_size_16_color_FFFFFF_t_70 3]  
[C语言的一些矩阵运算][C]

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQ2ODc3Njk3_size_16_color_FFFFFF_t_70]: /images/20221021/0ede2a2377144dc5be8c3bd2f90508f6.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQ2ODc3Njk3_size_16_color_FFFFFF_t_70 1]: /images/20221021/f3cf2691d81a40c08048391940aea9c7.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQ2ODc3Njk3_size_16_color_FFFFFF_t_70 2]: /images/20221021/39590ffcca544d1b8349451c0ed7d4c2.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQ2ODc3Njk3_size_16_color_FFFFFF_t_70 3]: /images/20221021/ae5497e7898f40b78c852514cabb70ad.png
[C]: https://blog.csdn.net/qq_46877697/article/details/116073981