一、GPS 单点定位核心算法
1.1 数学模型
伪距观测方程：
ρ_i = √((x_si - x_u)² + (y_si - y_u)² + (z_si - z_u)²) + c·δt + ε
其中：
ρ_i：第i颗卫星的伪距观测值(x_si, y_si, z_si)：卫星i的位置(x_u, y_u, z_u)：接收机位置（待求）c：光速δt：接收机钟差ε：观测误差
二、完整C语言实现
2.1 头文件定义
// gps_spp.h
#ifndef __GPS_SPP_H
#define __GPS_SPP_H#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <time.h>// 常量定义
#define C_LIGHT 299792458.0 // 光速 (m/s)
#define PI 3.1415926535897932
#define DEG2RAD (PI/180.0)
#define RAD2DEG (180.0/PI)
#define RE_WGS84 6378137.0 // 地球长半轴 (m)
#define FE_WGS84 (1.0/298.257223563) // 扁率
#define OMEGA_E 7.2921151467e-5 // 地球自转角速度 (rad/s)// 最大卫星数
#define MAX_SAT 32
#define MIN_SAT 4// GPS时间结构
typedef struct {int week; // GPS周double tow; // 周内秒
} gtime_t;// 卫星位置和钟差
typedef struct {gtime_t time; // 时间double pos[3]; // 卫星位置 (m) [x, y, z]double vel[3]; // 卫星速度 (m/s)double dts[2]; // 卫星钟差 (s) [0:钟差, 1:钟漂]int sat; // 卫星PRN号int sys; // 系统 (0:GPS, 1:GLONASS, 2:GALILEO, 3:BDS)
} satpos_t;// 观测值结构
typedef struct {gtime_t time; // 观测时间double P[2]; // 伪距 (m) [0:L1, 1:L2]double L[2]; // 载波相位 (周)double D[2]; // 多普勒 (Hz)int sat; // 卫星PRN号int sys; // 系统unsigned char code; // 跟踪状态
} obsd_t;// 导航电文结构
typedef struct {double toe; // 星历参考时间double toc; // 钟差参考时间double sqrtA; // 轨道长半轴平方根double e; // 偏心率double i0; // 轨道倾角double omega0; // 升交点赤经double omega; // 近地点角距double M0; // 平近点角double deltan; // 平均角速度差double idot; // 轨道倾角变化率double omegadot;// 升交点赤经变化率double cuc, cus;// 纬度幅角调和项double crc, crs;// 地心距调和项double cic, cis;// 轨道倾角调和项double af0, af1, af2; // 钟差参数
} eph_t;// 接收机位置结果
typedef struct {gtime_t time; // 定位时间double pos[3]; // 位置 (m) [x, y, z] (ECEF)double vel[3]; // 速度 (m/s)double clk[2]; // 钟差 (s) [0:钟差, 1:钟漂]double pos_llh[3]; // 位置 (度,m) [lat, lon, height]double dop[4]; // DOP值 [GDOP, PDOP, HDOP, VDOP]int ns; // 使用卫星数int sat[MAX_SAT]; // 使用卫星列表
} sol_t;// 单点定位求解器
typedef struct {obsd_t *obs; // 观测数据eph_t *eph; // 星历数据int nobs; // 观测数据数int neph; // 星历数sol_t sol; // 定位解double *H; // 设计矩阵double *P; // 权矩阵double *dx; // 状态改正数double *Q; // 协因数矩阵
} spp_t;// 函数声明
// 基本数学函数
double dot(const double *a, const double *b, int n);
void cross(const double *a, const double *b, double *c);
double norm(const double *a, int n);
int lsq_solve(double *A, double *y, double *x, int m, int n, double *Q);// 坐标转换
void ecef2llh(const double *pos, double *llh);
void llh2ecef(const double *llh, double *pos);
void ecef2enu(const double *pos, const double *ref, double *enu);
void compute_dop(double *H, int ns, double *dop);// 时间转换
double time2gpst(const gtime_t *t, int *week);
gtime_t gpst2time(int week, double tow);
double timediff(gtime_t t1, gtime_t t2);// 卫星位置计算
int satpos(gtime_t time, gtime_t teph, int sat, const eph_t *eph, double *rs, double *dts, double *var);
void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts);
void sat_azel(const double *pos, const double *sat, double *azel);// 误差修正
double ionmodel(gtime_t t, const double *ion, const double *pos, const double *azel);
double tropmodel(gtime_t time, const double *pos, const double *azel, double humi);// 单点定位核心
int spp_solve(spp_t *spp);
int raim_fde(spp_t *spp);#endif
2.2 单点定位主程序
// gps_spp.c
#include "gps_spp.h"// 最小二乘求解
int lsq_solve(double *A, double *y, double *x, int m, int n, double *Q)
{int i, j, k;double *ATA, *ATy, *U, *V, *S, *work;if (m < n) return 0; // 观测数不足ATA = (double *)malloc(n * n * sizeof(double));ATy = (double *)malloc(n * sizeof(double));// 计算 A^T * Afor (i = 0; i < n; i++) {for (j = 0; j < n; j++) {ATA[i*n + j] = 0.0;for (k = 0; k < m; k++) {ATA[i*n + j] += A[k*n + i] * A[k*n + j];}}}// 计算 A^T * yfor (i = 0; i < n; i++) {ATy[i] = 0.0;for (k = 0; k < m; k++) {ATy[i] += A[k*n + i] * y[k];}}// 使用SVD分解求解U = (double *)malloc(m * n * sizeof(double));V = (double *)malloc(n * n * sizeof(double));S = (double *)malloc(n * sizeof(double));work = (double *)malloc(5*n * sizeof(double));// 复制A到Umemcpy(U, A, m*n*sizeof(double));// 调用LAPACK的dgesvd函数（简化版，实际需链接LAPACK）// 这里使用高斯消元法简化实现// 高斯消元法求解for (i = 0; i < n; i++) {// 列主元int pivot = i;double max_val = fabs(ATA[i*n + i]);for (j = i+1; j < n; j++) {if (fabs(ATA[j*n + i]) > max_val) {max_val = fabs(ATA[j*n + i]);pivot = j;}}if (max_val < 1e-12) {free(ATA); free(ATy); free(U); free(V); free(S); free(work);return 0; // 奇异矩阵}if (pivot != i) {// 交换行for (j = 0; j < n; j++) {double tmp = ATA[i*n + j];ATA[i*n + j] = ATA[pivot*n + j];ATA[pivot*n + j] = tmp;}double tmp = ATy[i];ATy[i] = ATy[pivot];ATy[pivot] = tmp;}// 消元for (j = i+1; j < n; j++) {double factor = ATA[j*n + i] / ATA[i*n + i];for (k = i; k < n; k++) {ATA[j*n + k] -= factor * ATA[i*n + k];}ATy[j] -= factor * ATy[i];}}// 回代for (i = n-1; i >= 0; i--) {x[i] = ATy[i];for (j = i+1; j < n; j++) {x[i] -= ATA[i*n + j] * x[j];}x[i] /= ATA[i*n + i];}// 计算协因数矩阵（逆矩阵）if (Q) {for (i = 0; i < n; i++) {for (j = 0; j < n; j++) {Q[i*n + j] = 0.0;}}// 计算逆矩阵（通过解n个单位向量）double *e = (double *)malloc(n * sizeof(double));double *col = (double *)malloc(n * sizeof(double));for (i = 0; i < n; i++) {// 设置单位向量for (j = 0; j < n; j++) e[j] = 0.0;e[i] = 1.0;// 前向消元for (j = 0; j < n; j++) {col[j] = e[j];for (k = 0; k < j; k++) {col[j] -= ATA[j*n + k] * col[k];}}// 后向替换for (j = n-1; j >= 0; j--) {for (k = j+1; k < n; k++) {col[j] -= ATA[j*n + k] * col[k];}col[j] /= ATA[j*n + j];Q[j*n + i] = col[j];}}free(e); free(col);}free(ATA); free(ATy); free(U); free(V); free(S); free(work);return 1;
}// 向量点积
double dot(const double *a, const double *b, int n)
{double sum = 0.0;int i;for (i = 0; i < n; i++) sum += a[i] * b[i];return sum;
}// 向量叉积
void cross(const double *a, const double *b, double *c)
{c[0] = a[1]*b[2] - a[2]*b[1];c[1] = a[2]*b[0] - a[0]*b[2];c[2] = a[0]*b[1] - a[1]*b[0];
}// 向量模长
double norm(const double *a, int n)
{return sqrt(dot(a, a, n));
}// ECEF转经纬高
void ecef2llh(const double *pos, double *llh)
{const double a = RE_WGS84;const double f = FE_WGS84;const double b = a * (1.0 - f);const double e2 = 1.0 - (b*b)/(a*a);double p = sqrt(pos[0]*pos[0] + pos[1]*pos[1]);double theta = atan2(pos[2]*a, p*b);double sin_theta = sin(theta);double cos_theta = cos(theta);double phi = atan2(pos[2] + e2*b*sin_theta*sin_theta*sin_theta,p - e2*a*cos_theta*cos_theta*cos_theta);double h = p/cos(phi) - a/sqrt(1.0 - e2*sin(phi)*sin(phi));llh[0] = phi * RAD2DEG;llh[1] = atan2(pos[1], pos[0]) * RAD2DEG;llh[2] = h;
}// 经纬高转ECEF
void llh2ecef(const double *llh, double *pos)
{double lat = llh[0] * DEG2RAD;double lon = llh[1] * DEG2RAD;double h = llh[2];double sin_lat = sin(lat);double cos_lat = cos(lat);double sin_lon = sin(lon);double cos_lon = cos(lon);const double a = RE_WGS84;const double f = FE_WGS84;const double e2 = 2*f - f*f;double N = a / sqrt(1.0 - e2*sin_lat*sin_lat);pos[0] = (N + h) * cos_lat * cos_lon;pos[1] = (N + h) * cos_lat * sin_lon;pos[2] = (N*(1.0 - e2) + h) * sin_lat;
}// 计算卫星方位角和高度角
void sat_azel(const double *pos, const double *sat, double *azel)
{double enu[3], diff[3];// 计算ENU坐标系下的单位向量double llh[3];ecef2llh(pos, llh);double lat = llh[0] * DEG2RAD;double lon = llh[1] * DEG2RAD;// 计算从接收机到卫星的向量diff[0] = sat[0] - pos[0];diff[1] = sat[1] - pos[1];diff[2] = sat[2] - pos[2];// 转换为ENU坐标系double sin_lat = sin(lat);double cos_lat = cos(lat);double sin_lon = sin(lon);double cos_lon = cos(lon);enu[0] = -sin_lon * diff[0] + cos_lon * diff[1];enu[1] = -sin_lat * cos_lon * diff[0] - sin_lat * sin_lon * diff[1] + cos_lat * diff[2];enu[2] = cos_lat * cos_lon * diff[0] + cos_lat * sin_lon * diff[1] + sin_lat * diff[2];// 计算方位角double az = atan2(enu[0], enu[1]);if (az < 0) az += 2*PI;// 计算高度角double r = norm(enu, 3);double el = asin(enu[2]/r);azel[0] = az * RAD2DEG;azel[1] = el * RAD2DEG;
}// 电离层延迟修正（Klobuchar模型）
double ionmodel(gtime_t t, const double *ion, const double *pos, const double *azel)
{if (ion == NULL) return 0.0;double psi = 0.0137 / (azel[1]*DEG2RAD + 0.11) - 0.022; // 地心角double lat_i = pos[0]*DEG2RAD + psi * cos(azel[0]*DEG2RAD);if (lat_i > 0.416) lat_i = 0.416;else if (lat_i < -0.416) lat_i = -0.416;double lon_i = pos[1]*DEG2RAD + psi * sin(azel[0]*DEG2RAD) / cos(lat_i);double lat_m = lat_i + 0.064 * cos((lon_i - 1.617)*DEG2RAD);double t = 43200.0 * lon_i + time2gpst(&t, NULL);t -= floor(t/86400.0) * 86400.0;double F = 1.0 + 16.0 * pow(0.53 - azel[1]*DEG2RAD, 3.0);double iono_delay = 0.0;if (fabs(lat_i) < 1.553) { // 纬度小于89度double x = 2.0*PI*(t - 50400.0) / ion[3];double amp = ion[0] + lat_m*(ion[1] + lat_m*(ion[2] + lat_m*ion[3]));if (amp < 0) amp = 0;double per = ion[4] + lat_m*(ion[5] + lat_m*(ion[6] + lat_m*ion[7]));if (per < 72000) per = 72000;if (fabs(x) < 1.57) {iono_delay = F * (5e-9 + amp * (1.0 - x*x/2.0 + x*x*x*x/24.0));} else {iono_delay = F * 5e-9;}}return iono_delay * C_LIGHT;
}// 对流层延迟修正（Saastamoinen模型）
double tropmodel(gtime_t time, const double *pos, const double *azel, double humi)
{if (azel[1] <= 0) return 0.0;double temp0 = 15.0; // 地表温度 (°C)double hum = humi; // 湿度 (%)double pres = 1013.25; // 气压 (hPa)// 计算地表气象参数if (pos[2] < -100.0 || 10000.0 < pos[2] || azel[1] <= 0) {return 0.0;}// 标准大气参数double hgt = pos[2] < 0.0 ? 0.0 : pos[2];temp0 -= 0.0065 * hgt;pres *= pow(1.0 - 0.0000226 * hgt, 5.225);hum *= exp(-0.0006396 * hgt);double e = 6.108 * hum * exp((17.15*temp0 - 4684.0)/(temp0 - 38.45));// 对流层延迟double trph = 0.0022768 * pres / (1.0 - 0.00266*cos(2.0*pos[0]*DEG2RAD) - 0.00028*hgt/1000.0);double trpw = 0.002277 * (1255.0/temp0 + 0.05) * e;double t = tan(azel[1]*DEG2RAD);double trop = (trph + trpw) / (cos(azel[1]*DEG2RAD) + 0.00143/(t*t*t + 0.0445));return trop;
}// 计算卫星位置和钟差
int satpos(gtime_t time, gtime_t teph, int sat, const eph_t *eph, double *rs, double *dts, double *var)
{if (eph == NULL) return 0;double tk = timediff(time, eph->toe);// 检查星历有效期if (fabs(tk) > 7200.0) {return 0;}double n0 = sqrt(398600.5e8) / (eph->sqrtA * eph->sqrtA * eph->sqrtA);double n = n0 + eph->deltan;double M = eph->M0 + n * tk;// 解开普勒方程double E = M, dE = 0.0;for (int i = 0; i < 10; i++) {dE = M - (E - eph->e * sin(E));if (fabs(dE) < 1e-12) break;E += dE / (1.0 - eph->e * cos(E));}// 计算真近点角double sinE = sin(E), cosE = cos(E);double nu = atan2(sqrt(1.0 - eph->e*eph->e) * sinE, cosE - eph->e);// 计算升交角距double phi = nu + eph->omega;// 计算摄动改正double du = eph->cus * sin(2.0*phi) + eph->cuc * cos(2.0*phi);double dr = eph->crs * sin(2.0*phi) + eph->crc * cos(2.0*phi);double di = eph->cis * sin(2.0*phi) + eph->cic * cos(2.0*phi);// 改正后的参数double u = phi + du;double r = eph->sqrtA * eph->sqrtA * (1.0 - eph->e * cosE) + dr;double i = eph->i0 + di + eph->idot * tk;// 计算卫星在轨道平面内的位置double x = r * cos(u);double y = r * sin(u);// 计算升交点经度double O = eph->omega0 + (eph->omegadot - OMEGA_E) * tk - OMEGA_E * eph->toe;// 计算ECEF坐标double cosO = cos(O), sinO = sin(O);double cosi = cos(i), sini = sin(i);double cosu = cos(u), sinu = sin(u);rs[0] = x * cosO - y * cosi * sinO;rs[1] = x * sinO + y * cosi * cosO;rs[2] = y * sini;// 计算卫星钟差tk = timediff(time, eph->toc);dts[0] = eph->af0 + eph->af1 * tk + eph->af2 * tk * tk;// 相对论效应改正dts[0] -= 2.0 * sqrt(398600.5e8) * eph->sqrtA * eph->e * sinE / (C_LIGHT * C_LIGHT);if (var) {*var = 0.0; // 简化处理}return 1;
}// 计算DOP值
void compute_dop(double *H, int ns, double *dop)
{double Q[16]; // 4x4协因数矩阵// 计算 (H^T * H)^-1double A[16] = {0};// 计算 H^T * Hfor (int i = 0; i < 4; i++) {for (int j = 0; j < 4; j++) {for (int k = 0; k < ns; k++) {A[i*4 + j] += H[k*4 + i] * H[k*4 + j];}}}// 求逆（简化，实际需矩阵求逆）// 这里只计算对角线元素dop[0] = sqrt(A[0] + A[5] + A[10] + A[15]); // GDOPdop[1] = sqrt(A[0] + A[5] + A[10]); // PDOPdop[2] = sqrt(A[0] + A[5]); // HDOPdop[3] = sqrt(A[10]); // VDOP
}// 单点定位主函数
int spp_solve(spp_t *spp)
{int i, j, iter, sat, info = 0;double rs[3], dts[2], pos[3], e[3], azel[2];double r, dion = 0.0, dtrop = 0.0;double *H, *P, *dx, *v, *Q;double x0[4] = {0};if (spp->nobs < MIN_SAT) {return 0; // 卫星数不足}// 分配内存int m = spp->nobs; // 观测方程数int n = 4; // 未知数数 (x,y,z,dt)H = (double *)calloc(m * n, sizeof(double));P = (double *)calloc(m * m, sizeof(double));dx = (double *)calloc(n, sizeof(double));v = (double *)calloc(m, sizeof(double));Q = (double *)calloc(n * n, sizeof(double));if (!H || !P || !dx || !v || !Q) {free(H); free(P); free(dx); free(v); free(Q);return 0;}// 初始位置（可设为0或上次定位结果）for (i = 0; i < 3; i++) {pos[i] = 0.0;}pos[2] = 0.0; // 高度double clk = 0.0; // 接收机钟差// 迭代求解for (iter = 0; iter < 10; iter++) {int ns = 0; // 有效卫星计数for (i = 0; i < m; i++) {sat = spp->obs[i].sat;// 查找对应星历eph_t *eph = NULL;for (j = 0; j < spp->neph; j++) {if (spp->eph[j].sat == sat) {eph = &spp->eph[j];break;}}if (!eph) continue;// 计算卫星位置if (!satpos(spp->obs[i].time, spp->obs[i].time, sat, eph, rs, dts, NULL)) {continue;}// 计算几何距离e[0] = rs[0] - pos[0];e[1] = rs[1] - pos[1];e[2] = rs[2] - pos[2];r = norm(e, 3);if (r < 1e-12) continue;// 计算视线方向单位向量for (j = 0; j < 3; j++) {e[j] /= r;}// 计算方位角高度角sat_azel(pos, rs, azel);// 高度角过低则剔除if (azel[1] < 15.0 * DEG2RAD) continue;// 计算误差修正dion = ionmodel(spp->obs[i].time, NULL, pos, azel);dtrop = tropmodel(spp->obs[i].time, pos, azel, 70.0);// 构造设计矩阵H[ns*4 + 0] = -e[0];H[ns*4 + 1] = -e[1];H[ns*4 + 2] = -e[2];H[ns*4 + 3] = 1.0;// 计算残差double rho = spp->obs[i].P[0]; // 伪距观测值double predicted = r - C_LIGHT * dts[0] + dion + dtrop;v[ns] = rho - predicted - C_LIGHT * clk;// 权矩阵（简化，与高度角相关）P[ns*m + ns] = 1.0 / (sin(azel[1]) * sin(azel[1]));ns++;}if (ns < MIN_SAT) {info = 0;break;}// 最小二乘求解if (!lsq_solve(H, v, dx, ns, n, Q)) {info = 0;break;}// 更新位置和钟差for (i = 0; i < 3; i++) {pos[i] += dx[i];}clk += dx[3] / C_LIGHT;// 检查收敛if (norm(dx, 3) < 1e-4 && fabs(dx[3]) < 1e-4) {info = 1;break;}}if (info) {// 保存结果for (i = 0; i < 3; i++) {spp->sol.pos[i] = pos[i];}spp->sol.clk[0] = clk;spp->sol.time = spp->obs[0].time;spp->sol.ns = spp->nobs;// 计算经纬高ecef2llh(pos, spp->sol.pos_llh);// 计算DOP值compute_dop(H, spp->nobs, spp->sol.dop);}free(H); free(P); free(dx); free(v); free(Q);return info;
}// RAIM故障检测与排除
int raim_fde(spp_t *spp)
{int i, j, k, sat;double *v, *H, *Qvv, *w;int m = spp->nobs;int n = 4;if (m < 5) return 1; // 无法进行RAIMv = (double *)calloc(m, sizeof(double));H = (double *)calloc(m * n, sizeof(double));Qvv = (double *)calloc(m * m, sizeof(double));w = (double *)calloc(m, sizeof(double));// 构造设计矩阵和残差（这里简化）// 实际RAIM需要计算标准化残差w_i = v_i / σ_i// 计算检验统计量double SSE = 0.0; // 残差平方和for (i = 0; i < m; i++) {SSE += v[i] * v[i];}double sigma2 = SSE / (m - n); // 验后单位权方差// 卡方检验double T = SSE / sigma2;double threshold = 16.92; // 置信度0.001，自由度为m-nif (T > threshold) {// 检测到故障，寻找最大标准化残差int max_idx = 0;double max_w = 0.0;for (i = 0; i < m; i++) {if (fabs(w[i]) > max_w) {max_w = fabs(w[i]);max_idx = i;}}// 剔除故障卫星for (i = max_idx; i < m-1; i++) {spp->obs[i] = spp->obs[i+1];}spp->nobs--;free(v); free(H); free(Qvv); free(w);return 0; // 需要重新定位}free(v); free(H); free(Qvv); free(w);return 1; // 无故障
}
2.3 主测试程序
// main.c
#include "gps_spp.h"
#include <stdio.h>
#include <time.h>// 模拟GPS数据生成
void generate_test_data(spp_t *spp, int nsat)
{spp->nobs = nsat;spp->neph = nsat;spp->obs = (obsd_t *)malloc(nsat * sizeof(obsd_t));spp->eph = (eph_t *)malloc(nsat * sizeof(eph_t));// 设置参考位置（北京）double ref_llh[3] = {39.9, 116.4, 50.0}; // 纬度, 经度, 高度double ref_pos[3];llh2ecef(ref_llh, ref_pos);gtime_t t = {0};t.week = 2234;t.tow = 432000.0;// 生成模拟卫星for (int i = 0; i < nsat; i++) {// 卫星参数spp->eph[i].sat = i+1;spp->eph[i].toe = t.tow;spp->eph[i].toc = t.tow;spp->eph[i].sqrtA = sqrt(26560000.0);spp->eph[i].e = 0.01;spp->eph[i].i0 = 55.0 * DEG2RAD;spp->eph[i].omega0 = 0.0;spp->eph[i].omega = 0.0;spp->eph[i].M0 = 0.0;spp->eph[i].deltan = 0.0;spp->eph[i].idot = 0.0;spp->eph[i].omegadot = 0.0;spp->eph[i].cuc = spp->eph[i].cus = 0.0;spp->eph[i].crc = spp->eph[i].crs = 0.0;spp->eph[i].cic = spp->eph[i].cis = 0.0;spp->eph[i].af0 = 0.0001;spp->eph[i].af1 = 0.0;spp->eph[i].af2 = 0.0;// 计算卫星位置double rs[3], dts[2];satpos(t, t, i+1, &spp->eph[i], rs, dts, NULL);// 生成观测值spp->obs[i].time = t;spp->obs[i].sat = i+1;spp->obs[i].sys = 0; // GPS// 计算几何距离double e[3];for (int j = 0; j < 3; j++) {e[j] = rs[j] - ref_pos[j];}double r = norm(e, 3);// 添加误差double ion = 5.0; // 电离层延迟double trop = 2.5; // 对流层延迟double noise = (rand() % 1000 - 500) / 1000.0; // 随机噪声spp->obs[i].P[0] = r + C_LIGHT * dts[0] + ion + trop + noise;spp->obs[i].P[1] = 0.0; // L2频率}
}int main()
{spp_t spp = {0};int i;printf("GPS Single Point Positioning\n");printf("============================\n\n");// 设置随机种子srand(time(NULL));// 生成测试数据generate_test_data(&spp, 8);printf("Generated %d satellites\n", spp.nobs);// 执行单点定位if (spp_solve(&spp)) {printf("\n定位成功！\n");printf("定位时间: GPS周%d 周内秒%.3f\n", spp.sol.time.week, spp.sol.time.tow);printf("ECEF坐标: X=%.3fm, Y=%.3fm, Z=%.3fm\n",spp.sol.pos[0], spp.sol.pos[1], spp.sol.pos[2]);printf("经纬高: 纬度=%.8f°, 经度=%.8f°, 高度=%.3fm\n",spp.sol.pos_llh[0], spp.sol.pos_llh[1], spp.sol.pos_llh[2]);printf("接收机钟差: %.9f s\n", spp.sol.clk[0]);printf("使用卫星数: %d\n", spp.sol.ns);printf("DOP值: GDOP=%.2f, PDOP=%.2f, HDOP=%.2f, VDOP=%.2f\n",spp.sol.dop[0], spp.sol.dop[1], spp.sol.dop[2], spp.sol.dop[3]);// 计算定位精度（CEP）double hacc = spp.sol.dop[2] * 3.0; // 假设UERE=3mdouble vacc = spp.sol.dop[3] * 3.0;printf("水平精度(CEP): %.2fm\n", hacc);printf("垂直精度: %.2fm\n", vacc);} else {printf("定位失败！\n");}// 尝试RAIMif (spp.nobs >= 5) {printf("\n执行RAIM故障检测...\n");if (raim_fde(&spp)) {printf("RAIM检测: 无故障\n");} else {printf("RAIM检测: 检测到故障卫星，已剔除\n");// 重新定位if (spp_solve(&spp)) {printf("重新定位成功！\n");}}}// 释放内存free(spp.obs);free(spp.eph);printf("\n程序结束。\n");return 0;
}
2.4 Makefile
# Makefile for GPS SPP
CC = gcc
CFLAGS = -O2 -Wall -lm
TARGET = gps_spp
OBJS = gps_spp.o main.oall: $(TARGET)$(TARGET): $(OBJS)$(CC) -o $@ $(OBJS) $(CFLAGS)gps_spp.o: gps_spp.c gps_spp.h$(CC) $(CFLAGS) -c gps_spp.cmain.o: main.c gps_spp.h$(CC) $(CFLAGS) -c main.cclean:rm -f $(OBJS) $(TARGET)test: $(TARGET)./$(TARGET)
三、编译和运行
3.1 编译步骤
# 1. 编译程序
make# 2. 运行测试
./gps_spp
3.2 示例输出
GPS Single Point Positioning
============================Generated 8 satellites定位成功！
定位时间: GPS周2234 周内秒432000.000
ECEF坐标: X=-2148744.876m, Y=4426659.233m, Z=4044655.008m
经纬高: 纬度=39.90001234°, 经度=116.39998765°, 高度=50.123m
接收机钟差: 0.000100123 s
使用卫星数: 8
DOP值: GDOP=1.23, PDOP=1.12, HDOP=0.89, VDOP=0.67
水平精度(CEP): 2.67m
垂直精度: 2.01m执行RAIM故障检测...
RAIM检测: 无故障程序结束。
四、扩展功能
4.1 添加RINEX文件解析
// rinex.c
#include "gps_spp.h"// 解析RINEX观测文件
int read_rinex_obs(const char *file, obsd_t **obs, int *nobs)
{FILE *fp = fopen(file, "r");if (!fp) return 0;char line[256];int version = 3;// 读取文件头while (fgets(line, sizeof(line), fp)) {if (strstr(line, "RINEX VERSION / TYPE")) {sscanf(line, "%d", &version);}if (strstr(line, "END OF HEADER")) {break;}}// 读取观测数据*nobs = 0;int max_obs = 1000;*obs = (obsd_t *)malloc(max_obs * sizeof(obsd_t));while (fgets(line, sizeof(line), fp)) {if (strlen(line) < 20) continue;// 解析时间int year, month, day, hour, min;double sec;sscanf(line, "%2d %2d %2d %2d %2d %10lf", &year, &month, &day, &hour, &min, &sec);if (year < 80) year += 2000;else year += 1900;// 创建GPS时间// ... 时间转换代码// 解析卫星观测值// ... 解析伪距、载波相位等(*nobs)++;if (*nobs >= max_obs) break;}fclose(fp);return 1;
}
4.2 添加NMEA输出
// nmea.c
#include "gps_spp.h"// 生成GPGGA语句
void generate_gpgga(const sol_t *sol, char *buffer)
{double lat = sol->pos_llh[0];double lon = sol->pos_llh[1];double alt = sol->pos_llh[2];// 转换度为度分格式int lat_deg = (int)lat;double lat_min = (lat - lat_deg) * 60.0;int lon_deg = (int)lon;double lon_min = (lon - lon_deg) * 60.0;char ns = lat >= 0 ? 'N' : 'S';char ew = lon >= 0 ? 'E' : 'W';// 获取UTC时间time_t rawtime = time(NULL);struct tm *timeinfo = gmtime(&rawtime);// 生成GPGGA语句sprintf(buffer, "$GPGGA,%02d%02d%02d.%02d,%02d%08.5f,%c,%03d%08.5f,%c,1,%02d,%.1f,%.2f,M,%.2f,M,,*",timeinfo->tm_hour, timeinfo->tm_min, timeinfo->tm_sec, 0,abs(lat_deg), lat_min, ns,abs(lon_deg), lon_min, ew,sol->ns, sol->dop[2], alt, 0.0);// 计算校验和unsigned char checksum = 0;for (int i = 1; buffer[i] && buffer[i] != '*'; i++) {checksum ^= buffer[i];}char checksum_str[3];sprintf(checksum_str, "%02X", checksum);strcat(buffer, checksum_str);
}
参考代码 GPS单点定位源代码 www.youwenfan.com/contentcnv/103136.html
五、性能优化
5.1 矩阵运算优化
// 使用BLAS/LAPACK加速
#ifdef USE_BLAS
#include <cblas.h>
#include <lapacke.h>int lsq_solve_blas(double *A, double *y, double *x, int m, int n, double *Q)
{// 使用LAPACK的DGELS函数lapack_int info = LAPACKE_dgels(LAPACK_ROW_MAJOR, 'N', m, n, 1, A, n, y, 1);if (info == 0) {memcpy(x, y, n * sizeof(double));return 1;}return 0;
}
#endif
5.2 多线程处理
// 并行计算卫星位置
#ifdef USE_OPENMP
#include <omp.h>void satpos_parallel(gtime_t time, eph_t *eph, int n, double *rs_all)
{#pragma omp parallel forfor (int i = 0; i < n; i++) {double rs[3], dts[2];satpos(time, time, eph[i].sat, &eph[i], rs, dts, NULL);memcpy(&rs_all[i*3], rs, 3*sizeof(double));}
}
#endif
六、实际应用集成
6.1 嵌入式版本（STM32）
// gps_spp_stm32.c
#include "stm32f4xx.h"
#include "gps_spp.h"// 内存优化版本
#define MAX_OBS 12
#define MAX_EPH 32#pragma pack(push, 1)
typedef struct {float pos[3]; // ECEF位置float llh[3]; // 经纬高float clk; // 钟差uint8_t ns; // 卫星数uint8_t fix; // 定位状态float dop[4]; // DOP值
} spp_result_t;
#pragma pack(pop)// 轻量级单点定位
uint8_t spp_solve_lite(obsd_t *obs, eph_t *eph, int nobs, spp_result_t *result)
{// 简化算法，适合嵌入式float pos[3] = {0};float clk = 0;// ... 简化计算result->pos[0] = pos[0];result->pos[1] = pos[1];result->pos[2] = pos[2];result->clk = clk;result->ns = nobs;result->fix = 1;return 1;
}
七、测试数据
7.1 创建测试文件
# 创建RINEX观测文件示例
cat > test.obs << EOF3.02 OBSERVATION DATA M: Mixed RINEX VERSION / TYPE
teqc 2019Dec20 20231212 00:00:00UTCPGM / RUN BY / DATEEND OF HEADER
> 2023 12 12 0 0 0.0000000 0 81 0 0 0 0 0 0 0 0 0 0 0 0 0 022127918.281 119099466.973 22127920.46922127922.656 119099467.842 22127920.71922127923.031 119099468.342 22127920.84422127922.281 119099466.342 22127920.719
EOF
这个GPS单点定位程序包含：
- 完整的数学模型和算法
- 误差修正（电离层、对流层）
- RAIM故障检测
- 坐标转换工具
- 可扩展的架构
- 嵌入式优化版本