GNSS单点定位与测速3：卫星位置计算、电离层、Saastamonien模型对流层延迟解算、卫星钟差改正、单点定位、测速
本程序适用于导航工程专业本科生课程作业：单点定位与测速。
本节为单点定位与测速部分。
文章最末尾的函数SPPpos即为单点定位与测速实现的程序。
程序参数依据GPS L1载波 C/A码。
文章目录
- GNSS单点定位与测速3：卫星位置计算、电离层、Saastamonien模型对流层延迟解算、卫星钟差改正、单点定位、测速
- 单点定位satpos.h
- 依赖库包含与WGS84坐标系参数设置
- 从二进制消息数组中获取电离层参数getion
- 计算卫星方向角和高度角函数getazel
- 计算电离层延迟ionmodel
- 对流层延迟解算函数tropmodel
- 补充：通用时转unix时函数com2unixtime
- 从伪距数组中获取伪距函数gettobsandrho
- 获取以星历钟差改正数为基的钟差改正量getdts
- 卫星位置、速度、钟差、钟速计算函数getsatelliteposition
- 单点定位与多普勒测速所用的创建矩阵函数getXX
- 单点定位与单点多普勒测速结果结构体SppResult及其初始化函数get_SppResult
- 单点定位与多普勒测速函数SPPpos
- 结果输出PrintResult
- 结果保存函数fprintResult
单点定位satpos.h
单点定位部分函数数量较多，笔记与代码不一定完全一一对应。
依赖库包含与WGS84坐标系参数设置
需要特别注意的是，本示例程序所有参数以WGS-84为准，其他系统请自行转换。
//文件名：satpos
/*功能：单点定位相关函数*/
/*对流层，电离层延迟解算、卫星钟差计算、卫星方位角高度角计算、卫星位置计算、单点定位*/
#include <stdio.h>
#include <math.h>
#include <string.h>
#include"coordinatetrans.h"
#include"binaryread.h"
//#include"class4.h"
#include"matprocess.h"
#include <math.h>
#define OMGE 7.2921151467E-5
#define GM 3.9860047e14
using namespace std;
//#define f 1.0/298.257223563
#define a 6378137.0
#define pi 3.1415926535897932384626433832795
#define Fc -4.442807633e-10//相对论效应改正参数
从二进制消息数组中获取电离层参数getion
参数：数据数组fullodata[]，电离层延迟参数结果数组ion[8]，二进制消息报告epoch；对于"OEM7"板卡协议，还设计了针对它的重载，多一个字符串参数。
功能：如题。
特别说明：这里设计重载的原因是OEM7协议中电离层参数消息头长为28，UB482协议中为24，结构不同。在示例程序中还有一个重载来自卫星导航原理课程的代码，用以从RINEX格式的文件中获取参数。
//以下函数为从二进制消息中读取电离层延迟参数
void getion(unsigned char* fullodata, double* ion, breport epoch) {
int start = epoch.start, end = epoch.end;
ion[0] = bit2double(fullodata + start + 24);
ion[1] = bit2double(fullodata + start + 24 + 8);
ion[2] = bit2double(fullodata + start + 24 + 16);
ion[3] = bit2double(fullodata + start + 24 + 24);
ion[4] = bit2double(fullodata + start + 24 + 32);
ion[5] = bit2double(fullodata + start + 24 + 40);
ion[6] = bit2double(fullodata + start + 24 + 48);
ion[7] = bit2double(fullodata + start + 24 + 56);
}
void getion(unsigned char* fullodata, double* ion, breport epoch,const char* OEM7) {
int start = epoch.start, end = epoch.end;
ion[0] = bit2double(fullodata + start + 28);
ion[1] = bit2double(fullodata + start + 28 + 8);
ion[2] = bit2double(fullodata + start + 28 + 16);
ion[3] = bit2double(fullodata + start + 28 + 24);
ion[4] = bit2double(fullodata + start + 28 + 32);
ion[5] = bit2double(fullodata + start + 28 + 40);
ion[6] = bit2double(fullodata + start + 28 + 48);
ion[7] = bit2double(fullodata + start + 28 + 56);
}
计算卫星方向角和高度角函数getazel
参数：卫星位置数组rs[3]，接收机位置数组rr[3]，方位角高度角结果数组azel[2]。
功能：如题。
特别说明：在这个函数中，部分区域使用的角度为半圆角，注意弧度、角度和半圆角度之间的转换。
//以下函数功能为计算卫星方向角和高度角
void getazel(const double* rs, const double* rr, double* azel) {
//从参数接受卫星和接收机坐标同时转化为大地坐标
//double ion[3]={},pos[3]={};
double rsblh[3]; xyztoblh(rs[0], rs[1], rs[2], rsblh);
double rrblh[3]; xyztoblh(rr[0], rr[1], rr[2], rrblh);//错误点
//请注意xyztoblh函数返回值的格式是角度制还是弧度制，本代码为角度制
double d = sqrt(pow(rs[0] - rr[0], 2) + pow(rs[1] - rr[1], 2) + pow(rs[2] - rr[2], 2));
double e1, e2, e3, e[3];
e1 = (rs[0] - rr[0]) / d; e2 = (rs[1] - rr[1]) / d; e3 = (rs[2] - rr[2]) / d;
e[0] = rs[0] - rr[0]; e[1] = rs[1] - rr[1]; e[2] = rs[2] - rr[2];
double B = rrblh[0] * pi / 180, L = rrblh[1] * pi / 180;//测站的大地坐标，转换为弧度制以计算三角函数
//地心坐标系转站心坐标系
double E, N, U, ER[3] = {};
double H[9] = { -sin(L),cos(L),0,
-sin(B) * cos(L),-sin(B) * sin(L),cos(B),
cos(B) * cos(L),cos(B) * sin(L),sin(B) };
matx(H, 3, 3, e, 3, 1, ER);
E = ER[0]; N = ER[1]; U = ER[2];
//卫星高度角/方向角的计算
double az, el;
az = atan2(E, N);
el = asin(U / d);
azel[0] = az;
azel[1] = el;
}
计算电离层延迟ionmodel
参数：GPS时格式的观测时间t，电离层延迟参数数组ion[8]，接收机位置数组pos[3]，卫星高度角
方位角数组azel[2]。
功能：根据广播星历播发的电离层延迟参数计算电离层延迟。
特别说明：该函数以KLOBUCHAR模型计算电离层延迟。
//以下函数功能为计算电离层延迟
double ionmodel(GPSTime t, const double* ion, const double* pos, const double* azel) {
//从参数中取出需要的数值
double az = azel[0];
double el = azel[1];
double rrblh[3]; xyztoblh(pos[0], pos[1], pos[2], rrblh);
//计算地球中心角
double Phi = 0.0137 / ((el / pi) + 0.11) - 0.022;
//计算电离层穿刺点的纬度phi1
double phi1, phiu;
phiu = rrblh[0] / 180.0;
phi1 = phiu + Phi * cos(az);
if (phi1 > 0.416) phi1 = 0.416;
else if (phi1 < -0.416) phi1 = -0.416;
//计算电离层穿刺点经度lamda1
double lamda1, lamdau;
lamdau = rrblh[1] / 180.0;
lamda1 = lamdau + Phi * sin(az) / cos(phi1 * pi);
//计算电离层穿刺点的地磁纬度phim
double phim;
phim = phi1 + 0.064 * cos((lamda1 - 1.617) * pi);
//计算电离层穿刺点的当地时间localtime
double localtime;
localtime = 43200 * lamda1 + t.Second;//计算当地时（以GPS周内秒为基准）
localtime = localtime - floor(localtime / 86400.0) * 86400;//扣除整数天数，得到一天内的地方时秒数
//计算电离层延迟的幅度A1
double A1;
A1 = ion[0] + phim * (ion[1] + phim * (ion[2] + phim * ion[3]));
if (A1 < 0) A1 = 0;
//计算电离层延迟的周期P1
double P1;
P1 = ion[4] + phim * (ion[5] + phim * (ion[6] + phim * ion[7]));
if (P1 < 72000) P1 = 72000;
//计算电离层延迟相位X1
double X1;
X1 = 2 * pi * (localtime - 50400) / P1;
//计算倾斜因子F
double F;
F = 1.0 + 16.0 * pow((0.53 - el / pi), 3);
//模型参数计算完毕，下面根据模型计算电离层延迟IL1GPS
double IL1GPS;
if (fabs(X1) <= 1.57)
IL1GPS = clight * (5 * (1e-9) + A1 * (1 - 0.5 * X1 * X1 + pow(X1, 4) / 24.0)) * F;
else
IL1GPS = 5 * (1e-9) * clight * F;
double IGS1 = clight * (5 * (1e-9) + A1 * (1 - 0.5 * X1 * X1 + pow(X1, 4) / 24.0)) * F;
double IGS2 = 5 * (1e-9) * clight * F;
return IL1GPS;
}
对流层延迟解算函数tropmodel
参数：接收机位置数组pos[3]，卫星高度角方位角数组azel[2]。
功能：如题。
特别说明：该函数以Hopfield模型计算对流层延迟。
//以下函数功能为对流层延迟计算
double tropmodel(const double* pos, const double* azel) {
//将直角坐标转换为大地坐标
double posblh[3]; xyztoblh(pos[0], pos[1], pos[2], posblh);
//不在地球上，对流层延迟归零
if (posblh[2] < -100.0 || 1E4 < posblh[2] || azel[1] <= 0) return 0.0;
double humi = 0.7;
double h = posblh[2], b = posblh[0] * pi / 180.0;//因为在头文件中坐标转换函数定义为角度值，所以计算前需要复原
if (posblh[2] < 0.0) h = 0.0;//地面高程归零处理
double T = 15.0 - 6.5 * 1e-3 * h + 273.16;
double e = 6.108 * humi * exp((17.15 * T - 4684.0) / (T - 38.45));
double p = 1013.25 * pow((1 - 2.2557e-5 * h), 5.2568);
double z = pi / 2.0 - azel[1];
double trph = 0.0022768 * p / (cos(z) * (1.0 - 0.00266 * cos(2 * b) - 0.00028 * h / 1000.0));
double trpw = 0.002277 * (1255.0 / T + 0.05) * e / cos(z);
double trp = trph + trpw;
return trp;
/*卫星高度角、方向角、电离层延迟、对流层延迟计算函数结束*/
}
补充：通用时转unix时函数com2unixtime
该函数作为时间转换函数库字符串转unix时函数的一个补充。参数为通用时结构体，功能如题。
struct gtime_t com2unixtime(struct COMMONTIME t0) {
struct gtime_t t;
int i = 0;
//开始计算通用时距Unixtime起点时的Days
int Days = 0;
int doy[] = { 1,32,60,91,121,152,182,213,244,274,305,335 };
Days = (t0.Year - 1970) * 365 + (t0.Year - 1969) / 4 + doy[t0.Month - 1] + t0.Day - 2 + (t0.Year % 4 == 0 && t0.Month >= 3 ? 1 : 0);//老师提供算法，对2000年以前时间表现出暂时有误性
t.time = Days * 86400 + t0.Hour * 3600 + t0.Minute * 60 + floor(t0.Second);
t.second = t0.Second - floor(t0.Second);
return t;
}
从伪距数组中获取伪距函数gettobsandrho
参数：伪距观测值数组R，卫星所属国家，prn序号部分name，引用结果变量rho。
功能：从观测数组R中获取指定prn号卫星的伪距。
特别说明：该函数主要应用于从以prn号作为下标的伪距观测值数组中获取指定prn号卫星的伪距观测值。
//直接应用伪距数组(结构体)获取伪距
int gettobsandrho(double* R, char country, int name, double& rho) {
if (R[name] < 200000) { printf("未接收到%c%02d卫星信号\n", country, name); return 0; }//无对应伪距
else {
rho = R[name];
}
return 1;
}
获取以星历钟差改正数为基的钟差改正量getdts
参数：星历结构体s，观测时间rt。
功能：计算af0+af1*(rt-st)+af2*(rt-st)(rt-st)
特别说明：该函数不是最终输出的卫星钟差！！！，只是为了简化每次调用钟差参数时的写法。当对精度要求不高时，也可以直接当作卫星钟差使用。在示例程序中还有来自于原理课程的重载函数，用于rinex格式文件读取程序。
//计算卫星钟差
double getdts(eph_t s, gtime_t rt) {
double dts;
dts = s.af0 + s.af1 * double(rt.time + rt.second - s.toc.time - s.toc.second) + s.af2 * double(rt.time + rt.second - s.toc.time - s.toc.second) * double(rt.time + rt.second - s.toc.time - s.toc.second);
return dts;
}
卫星位置、速度、钟差、钟速计算函数getsatelliteposition
。
参数：卫星星历satellite，国家编号country，卫星prn序号name，伪距观测值rho，卫星位置结果数组xyz[3]，卫星速度结果数组dotxyzt[3]，观测时间rt，卫星星历参考时间st0，引用卫星钟差计算结果数组tdts，引用卫星钟速计算结果tdtss。
功能：计算卫星在地心地固坐标系下的位置(不含最终自转改正)、速度、钟差、钟速。
特别说明：该函数输出位置不含最终的自转改正(如与老师结果对照请在自转改正后对照)、该函数钟差不参与迭代、该函数偏近点角Ek参与迭代。需要特别注意的是，该函数相较于之前的版本有较大的增添，主要包括卫星速度的计算和钟差的计算，请注意对照代码。
void getsatelliteposition(eph_t satellite, char country, int name, double rho, double* xyz, double *dotxyz, gtime_t rt, gtime_t st0,double &tdts,double &tdtss) {
struct COMMONTIME comtime;
gtime_t tobs = rt;//观测时间
//计算卫星轨道长半轴
double A = satellite.A;
//计算平均运动角速度
double n0 = sqrt(GM / A / A / A);//GM为地球引力常数
//计算相对星历参考历元的时间
double t = tobs.time - rho / clight - getdts(satellite, tobs) + satellite.tgd;
double tk = t - satellite.toe.time;
if (tk > 302400) tk = tk - 604800;
else if (tk < -302400) tk = tk + 604800;
else tk = tk;
//对平均运动角速度进行改正Mk
double n = satellite.deln + n0;
//计算平进角点
double Mk = satellite.M0 + n * tk;
//计算偏近角点
double Ek = Mk;
double Ek1;
while (1) {
//Ek1=Ek-(Ek-satellite.e*sin(Ek)-Mk)/(1-satellite.e*cos(Ek));
Ek1 = Mk + satellite.e * sin(Ek);
if (abs(Ek1 - Ek) < 1e-13) break;
Ek = Ek1;
}
Ek = Ek1;
//计算真近点角vk
double vk = atan2(sqrt(1 - satellite.e * satellite.e) * sin(Ek) / (1 - satellite.e * cos(Ek)), (cos(Ek) - satellite.e) / (1 - satellite.e * cos(Ek)));
//计算升交点角距Phik
double Phik = vk + satellite.omg;
//计算二阶调和改正数
double deltauk = satellite.cus * sin(2 * Phik) + satellite.cuc * cos(2 * Phik);
double deltark = satellite.crs * sin(2 * Phik) + satellite.crc * cos(2 * Phik);
double deltaik = satellite.cis * sin(2 * Phik) + satellite.cic * cos(2 * Phik);
//计算改正的升交角距
double uk = Phik + deltauk;
//计算改正的向径
double rk = A * (1 - satellite.e * cos(Ek)) + deltark;
//计算改正的轨道倾角
double ik = satellite.i0 + deltaik + satellite.idot * tk;
//计算卫星在轨道平面上的位置
double xk1 = rk * cos(uk);
double yk1 = rk * sin(uk);
//计算经过改正的升交点经度
double Omegak = satellite.OMG0 + (satellite.OMGD - OMGE) * tk - OMGE * satellite.toes;
//计算卫星在地心地固坐标系下位置
double xk = xk1 * cos(Omegak) - yk1 * cos(ik) * sin(Omegak);
double yk = xk1 * sin(Omegak) + yk1 * cos(ik) * cos(Omegak);
double zk = yk1 * sin(ik);
xyz[0] = xk;
xyz[1] = yk;
xyz[2] = zk;
//以下为计算卫星速度
//计算偏近点角的时间导数
double dotEk = n / (1 - satellite.e*cos(Ek));
//计算真近点角的时间导数
double dotvk = sqrt((1+satellite.e)/(1-satellite.e))*cos(vk/2)*cos(vk/2)/cos(Ek/2)/cos(Ek/2)*dotEk;
//计算升交角距的时间导数
double dotuk = dotvk*(1+2*satellite.cus*cos(2*Phik)-2*satellite.cuc*sin(2*Phik));
//计算向径的时间导数
double dotrk = dotEk * A * satellite.e * sin(Ek) + 2*dotvk * (satellite.crs*cos(2*Phik)-satellite.crc*sin(2*Phik));
//计算轨道倾角的时间导数
double dotik = satellite.idot + 2 * dotvk * (satellite.cis*cos(2*Phik)-satellite.cic*sin(2*Phik));
//计算卫星轨道平面位置的时间导数
double dotxk = cos(uk) * dotrk - rk * sin(uk) * dotuk;
double dotyk = sin(uk) * dotrk + rk * cos(uk) * dotuk;
//计算卫星在地心坐标系下的速度并输出到结果数组
double dotR[12] = { cos(Omegak),-sin(Omegak) * cos(ik),-xk1 * sin(Omegak) - yk1 * cos(Omegak) * cos(ik), yk1 * sin(Omegak) * sin(ik),
sin(Omegak), cos(Omegak) * cos(ik), xk1 * cos(Omegak) - yk1 * sin(Omegak) * cos(ik),-yk1 * cos(Omegak) * sin(ik),
0 , sin(ik) , 0 , yk*cos(ik) };
double tx[4] = { dotxk,dotyk,satellite.OMGD - OMGE,dotik};
double dotxyzr[3] = {};
matx(dotR, 3, 4, tx, 4, 1, dotxyzr);
dotxyz[0]=dotxyzr[0], dotxyz[1]=dotxyzr[1], dotxyz[2]=dotxyzr[2];
//计算卫星钟差、钟速并输出到结果数组
tdts= getdts(satellite, tobs) + Fc * satellite.e * sqrt(satellite.A) * sin(Ek) - satellite.tgd;
tdtss = satellite.af1 + 2*satellite.af2 * (tobs.time + tobs.second - tdts - satellite.toc.time - satellite.toc.second)+Fc*satellite.e*sqrt(A)*cos(Ek)*dotEk;
}
单点定位与多普勒测速所用的创建矩阵函数getXX
本节展示单点定位与单点多普勒观测值测速过程中用到的矩阵(如：设计矩阵B，余弦方向矩阵l、m、n，定位观测值矩阵L，测速观测值矩阵w)的创建函数。
//单点定位用创建矩阵
double getR0(double* xyz0, double* xyz) {
double R0;
R0 = pow(xyz[0] - xyz0[0], 2) + pow(xyz[1] - xyz0[1], 2) + pow(xyz[2] - xyz0[2], 2);
R0 = sqrt(R0);
return R0;
}
double getl(double Xs, double X0, double R0) {
return (Xs - X0) / R0;
}
double getm(double Ys, double Y0, double R0) {
return (Ys - Y0) / R0;
}
double getn(double Zs, double Z0, double R0) {
return (Zs - Z0) / R0;
}
void getmatrixL(double* Pi, double* R0, double* dts, double* dion, double* trp, int n, double* L) {
int i;
for (i = 0; i < n; i++) {
L[i] = Pi[i] - R0[i] + clight * dts[i] - dion[i] - trp[i];
}
}
void getmatrixB(double* l, double* m, double* n, int num, double* B) {
int i;
for (i = 0; i < num; i++) {
B[4 * i + 0] = -l[i];
B[4 * i + 1] = -m[i];
B[4 * i + 2] = -n[i];
B[4 * i + 3] = 1;
}
}
void getmatrixP(double* var, int num, double* P) {
for (int i = 0; i < num; i++)
P[i * num + i] = 1.0 / var[i];
}
单点定位与单点多普勒测速结果结构体SppResult及其初始化函数get_SppResult
该结构体用于储存单点定位与测速结果。该函数用于在定位程序中获取结果(仅用于简化代码)。
typedef struct {
char sysytem[20] = { "GPS" };//默认为GPS系统解算坐标
double xyzt[4] = {};//定位结果
double xyztspeed[4] = {};//测速结果
double PDOP = 0;//三位点位衰减
double TDOP = 0;//时间点位衰减
double GDOP = 0;//几何点位衰减
gtime_t tobs;
}SppResult;
SppResult get_SppResult(double Xr,double Yr,double Zr,double Dtr,double *dotxyzt,double pdot,double tdot,double gdot,gtime_t tobs) {
SppResult Result;
Result.xyzt[0] = Xr;Result.xyzt[1] = Yr;Result.xyzt[2] = Zr;Result.xyzt[3] = Dtr;
Result.xyztspeed[0] = dotxyzt[0]; Result.xyztspeed[1] = dotxyzt[1];
Result.xyztspeed[2] = dotxyzt[2]; Result.xyztspeed[3] = dotxyzt[3]/clight;
//除以clight只需要除一次，如果函数外已经处理过了函数内就不用处理了
Result.PDOP = pdot, Result.TDOP = tdot, Result.GDOP = gdot;
Result.tobs = tobs;
//本函数不改变定位系统名称，仍然保持默认为"GPS"
return Result;
}
单点定位与多普勒测速函数SPPpos
经过长达40余页示例程序的准备，我们即将来到最后一步–单点定位与多普勒测速函数的撰写。
首先需要说明的是，该函数相较于上个版本增加了测速部分并删除了在函数中直接书写冗长的定位结果的部分；同时相较于卫星导航原理课程以RINEX文件为基础的程序修改了一些重要的错误（如：结果输出时钟差数组索引超出限制、构建观测值矩阵时公式正负号错误），请一定、一定、一定要仔细对照!
下面简要叙述一下整个函数的结构。
-
首先，根据传入的卫星星历数组和观测值结构体，对可视卫星进行筛选，筛选后，保留其伪距、多普勒、prn序号和信噪比观测值。
-
其次，对筛选后的卫星做数量判断，若小于4，则不执行定位并返回0。
-
以32为最大观测通道数量，开始初始化计算过程中需要用到的变量，变量含义见表1.1{reference-type=“ref”
reference=“fig:1”} -
进入迭代计算循环while(1)，直到卫星位置的变化小于阈值保存结果并输出。
-
在迭代计算的循环中，首先循环获取各卫星的位置，钟差，钟速，几何距离，方向余弦，电离层对流层延迟，并设定权重模型。
-
随后构建用于最小二乘估计的设计矩阵，伪距观测值矩阵，多普勒观测值矩阵并进行最小二乘求解。需要注意的是，原始多普勒观测值以Hz为单位，需要与数据对应的载波波长相乘并取负转换为以m/s为单位的观测值。
-
循环直至接收机位置结果变化小于阈值，计算各精度指标(DOP)，保存结果。
-
按照喜欢的方式输出结果或保存到文件中。
下面还需要特别说明的关于卫星定权模型的几个问题。根据教材和部分文献，经典的定权方法主要有信噪比模型和高度角模型(已经书写在代码中)。同时，代码作者根据数据实际结算结果和实验，自行提出了以伪距观测值标准差为基础的算法，丝毫没有任何理论依据，慎重使用。各个模型对两份文件的效果已经提前测试完成，结果详见代码，同时也欢迎大家对定权模型进行修正。
//单点定位与多普勒测速
/*参数解析*/
/*R：伪距观测值结构体*/
/*satllite：星历结构体数组*/
/*ion：电离层延迟参数数组*/
/*Result：计算结果结构体*/
int SPPpos(GPSOBS R, eph_t* satllite, double* ion,SppResult &Result) {
int num = R.num; double USEFUL[36] = {}; int usenum = 0; float Cno[36] = {}; float psrstd[36] = {}; float dopp[36] = {};
int sname[36] = {};
for (int i = 0; i < num; i++)
if (R.name[i] <= 32&&satllite[R.name[i]].svh == 0&&satllite[R.name[i]].statu==EPHYES&&R.name[i]!=8)
USEFUL[usenum] = R.R0[i], sname[usenum] = R.name[i],Cno[usenum]=R.Cno[i],psrstd[usenum]=R.psrstd[i],dopp[usenum]=R.dopp[i], usenum++;
num = usenum;
//检查卫星数量
if (num < 4) {
printf("卫星数量太少，无法定位");
return 0;
}
//初始化观测时间
gtime_t tobs = R.rt;//观测时间
//初始化单点定位、测速变量
double dts[32] = {}, R0[32] = {}, l[32] = {}, m[32] = {}, n[32] = {}, xyz[3] = {}, Pi[32] = {}, xyz0[3] = { 100,100,100 }, dion[32] = {}, trp[32] = {}, var[32] = {};
double dtss[32] = {}, dotxyz[3] = {}, w[32] = {};
double PDOP, TDOP, GDOP, Xr, Yr, Zr, Dtr;
while (1) {
//参数获取
for (int i = 0; i < num; i++) {
//记录卫星编号并提取对应卫星的星历
int name = sname[i];
eph_t s = satllite[name];
gtime_t st0 = s.toe;
//记录name[i]卫星的伪距观测值
Pi[i] = USEFUL[i]; double rho = Pi[i];
//获取卫星位置与钟差
double tdts,tdtss;
getsatelliteposition(s, 'G', name, rho, xyz, dotxyz,tobs, st0, tdts,tdtss);
//获取name[i]号卫星的钟差钟速
dts[i] = tdts; dtss[i] = tdtss;
//地球自转改正
double alpha = OMGE * (rho / clight + dts[i]);
xyz[0] = cos(alpha) * xyz[0] + sin(alpha) * xyz[1];
xyz[1] = cos(alpha) * xyz[1] - sin(alpha) * xyz[0];
//计算几何距离R0，方向余弦
R0[i] = getR0(xyz0, xyz);
l[i] = getl(xyz[0], xyz0[0], R0[i]);
m[i] = getm(xyz[1], xyz0[1], R0[i]);
n[i] = getn(xyz[2], xyz0[2], R0[i]);
//计算多普勒观测值
double rorate = l[i] * dotxyz[0] + m[i] * dotxyz[1] + n[i] * dotxyz[2];
w[i] = -0.19029367*dopp[i] - (rorate) + clight * dtss[i];//L1频率波长19.03cm,L2频率波长24.42cm,L5频率波长25.48cm
//计算电离层,对流层延迟
double azel[2];
getazel(xyz, xyz0, azel);
struct GPSTime tobsgps; int week;
tobsgps.Second = time2gpst(tobs, week);
tobsgps.Week = week;
dion[i] = ionmodel(tobsgps, ion, xyz0, azel);
trp[i] = tropmodel(xyz0, azel);
//计算var(为构建P矩阵做准备
//var[i] = 1;//单位阵格式
//这个言之有理即可
var[i] = 0.00224*pow(10, -Cno[i] / 10);//信噪比模型1、载波L1参数为0.00224、L2参数为0.00077
/*其他定权模型欢迎补充*/
}
//构建设计矩阵
double B[32 * 4] = { 0 };
getmatrixB(l, m, n, num, B);
//组合观测值矩阵
double L[32 * 1];
getmatrixL(Pi, R0, dts, dion, trp, num, L);
//权重经验矩阵
double P[32 * 32] = { 0 };//权重经验矩阵
getmatrixP(var, num, P);
//计算矩阵Q
double BT[32 * 4] = {};
matT(B, num, 4, BT);
double A[4 * 32] = {};
matx(BT, 4, num, P, num, num, A);
double Qr[4 * 4] = {};
matx(A, 4, num, B, num, 4, Qr);
double Q[4 * 4] = { 0 };
inverseMatrix(Qr, 4, Q);
//计算位置、速度的最小二乘估计值
double X1[4 * 32] = {};
matx(Q, 4, 4, BT, 4, num, X1);
double X2[4 * 32] = {};
matx(X1, 4, num, P, num, num, X2);
double dxyzt[4] = {};
matx(X2, 4, num, L, num, 1, dxyzt);//位置估计值
double dotxyzt[4] = {};
matx(X2, 4, num, w, num, 1, dotxyzt);//速度估计值
//位置数值更新
xyz0[0] = xyz0[0] + dxyzt[0];
xyz0[1] = xyz0[1] + dxyzt[1];
xyz0[2] = xyz0[2] + dxyzt[2];
if (abs(dxyzt[0]) < 1e-4 && abs(dxyzt[1]) < 1e-4 && abs(dxyzt[2]) < 1e-4) {
//计算精度因子与接收机坐标、速度、钟差、钟速
PDOP = sqrt(Q[0] + Q[5] + Q[10]);
TDOP = sqrt(Q[15]);
GDOP = sqrt(Q[0] + Q[5] + Q[10] + Q[15]);
Xr = xyz0[0]; Yr = xyz0[1]; Zr = xyz0[2]; Dtr = dxyzt[3] / clight;
Result=get_SppResult(Xr, Yr, Zr, Dtr, dotxyzt, PDOP, TDOP, GDOP,tobs);
//printf("\n单点多普勒测速结果%lf %lf %lf %lf\n", dotxyzt[0], dotxyzt[1], dotxyzt[2], dotxyzt[3]/clight);
break;
}
}
/*
printf("根据卫星");
for (int j = 0; j < num; j++)
printf("G%02d ", sname[j]);
printf("定位结果如下：\n");
*/
return 1;
}
结果输出PrintResult
本函数，代码作者提供了三种模式的结果输出，参数分别为"simple"、“BLH”、“complex”。大家可以根据自己的喜好添加或删减。
void printResult(SppResult R,const char* mode="simple") {
if ( strstr(mode,"simple"))
{
printf("%lld\n%lf\t%lf\t%lf\t%.6e\n", R.tobs.time, R.xyzt[0], R.xyzt[1], R.xyzt[2], R.xyzt[3]);
printf("%lf\t%lf\t%lf\t%.6e\n", R.xyztspeed[0], R.xyztspeed[1], R.xyztspeed[2], R.xyztspeed[3]);
}
if (strstr(mode, "BLH"))
{
double blh[3] = {};
xyztoblh(R.xyzt[0], R.xyzt[1], R.xyzt[2], blh);
printf("%lld\n%lf\t%lf\t%lf\t%.6e\n", R.tobs.time, blh[0], blh[1], blh[2], R.xyzt[3]);
printf("%lf\t%lf\t%lf\t%.6e\n", R.xyztspeed[0], R.xyztspeed[1], R.xyztspeed[2], R.xyztspeed[3]);
}
if (strstr(mode, "complex"))
{
double blh[3] = {};
xyztoblh(R.xyzt[0], R.xyzt[1], R.xyzt[2], blh);
printf("观测时间:对准卫星时间%d年%d月%d日%d:%d:%.2lf\n", time2epoch(R.tobs).Year,
time2epoch(R.tobs).Month,
time2epoch(R.tobs).Day,
time2epoch(R.tobs).Hour,
time2epoch(R.tobs).Minute,
time2epoch(R.tobs).Second);
printf("X=%.6lf, Y=%.6lf, Z=%.6lf, 接收机钟差为：%.6es\n", R.xyzt[0], R.xyzt[1], R.xyzt[2], R.xyzt[3]);
printf("vX=%.6lf, vY=%.6lf, vZ=%.6lf, 接收机钟速为：%.6es\n", R.xyztspeed[0], R.xyztspeed[1], R.xyztspeed[2], R.xyztspeed[3]);
printf("B=%lf,L=%lf,H=%lf\n", blh[0], blh[1], blh[2]);
printf("三维点位精度衰减因子PDOP=%.6lf\n时间精度衰减因子TDOP=%.6lf\n几何精度衰减因子GDOP=%.6lf\n", R.PDOP, R.TDOP, R.GDOP);
}
}
结果保存函数fprintResult
参数：待保存到的文件指针fp，结果结构体R，重载一个字符串参数用于选择输出的坐标格式。
功能：把结果写入到fp指向的文件中。
//结果保存
void fprintResult(FILE* fp, SppResult R) {
fprintf(fp,"%lld\t%lf\t%lf\t%lf\t%.6e\t ", R.tobs.time, R.xyzt[0], R.xyzt[1], R.xyzt[2], R.xyzt[3]);
fprintf(fp,"%lf\t%lf\t%lf\t%.6e\n", R.xyztspeed[0], R.xyztspeed[1], R.xyztspeed[2], R.xyztspeed[3]);
}
void fprintResult(FILE* fp, SppResult R,const char *temp) {
double blh[3] = {};
xyztoblh(R.xyzt[0], R.xyzt[1], R.xyzt[2], blh);
fprintf(fp, "%lld\t%lf\t%lf\t%lf\t%.6e\n", R.tobs.time, blh[0], blh[1], blh[2], R.xyzt[3]);
}
3178
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
