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;
}
}
}
}
}
4.输入文件与输出结果结果
输入文件格式:
这里把所有的顶点的字母"P"去掉了
结果:
C语言的一些矩阵运算