MATLAB卫星定位解算：RINEX数据处理、最小二乘与EKF滤波

简介：面向GPS定位初学者的AMP MATLAB定位解算程序，围绕卫星导航中的几何定位问题，提供从数据读取到结果输出的完整示例代码，帮助用户理解伪距观测、卫星位置解算与最小二乘定位的基本逻辑。压缩包内共有五个文件，包括三个MATLAB脚本（分别负责单点位置解算、卫星位置速度计算、大地坐标转换）和两个文本格式数据文件（包含观测值文件和星历数据文件），压缩包整体大小为11KB。目前已有458人学习下载，适合测绘、导航或通信方向的本科生、研究生入门参考。代码结构和变量命名清晰，关键步骤均附有注释，从观测文件读取、误差修正到坐标输出，各阶段均有对应函数实现；涵盖数据预处理、电离层与对流层延迟校正、钟差估算、几何距离解算以及非线性最小二乘平差，借助观测值与星历数据可完整复现定位流程，也能在此基础上替换或增加函数模块，扩展至多星座联合定位或RTK研究，在提升编程能力的同时加深对卫星定位误差源和坐标转换细节的理解。

1. 解压完「卫星定位解算数据&程序.rar」之后先确认手里有什么

解压完这个 rar，多数人第一件事是找 main.m，然后直接 F5。这个标题其实在描述一类很典型的交付物：一批 GPS 原始观测数据，加上一套用 MATLAB 写的定位解算程序。AMP 在函数命名里出现时，通常是「自适应测量处理」的缩写，负责把信号质量转换成定位解的权重，不是 Google 那个 AMP，这一点先别理解偏。这篇文章会先把 RINEX 数据读进 MATLAB，用最小二乘把伪距定位解算跑通，再把 EKF 的 Q/R 配起来，最后落在输出坐标的验证方法上。适合课程设计、开题前的算法复现，也适合第一次拿到陌生定位代码包的工程师——你不需要猜作者把数据藏在了哪一列，按下面这套流程顺下来就能对上号。

2. 卫星定位解算的第一步：把观测数据读进 MATLAB

2.1 先分清两类数据：观测值文件和星历文件

GNSS 定位解算程序里，read_obs和read_nav基本是两条独立的链路，标题包里最常见的组合是.obs/.o观测文件配.nav/.n星历文件。观测文件按历元存了每颗卫星的伪距、载波相位、多普勒和信噪比 C/N0；星历文件给卫星轨道参数，解算顺序是先算卫星位置，再算接收机位置。

拿到包以后，我一般先看两个文件的前 20 行，确认 RINEX 版本。版本信息在头文件第一行：2.11和3.04的字段格式差很多，尤其是观测类型标识，3 系从C1改成了C1C、L1C这类三字符编号。不确认版本就写解析器，后面字段一律错位。还有一个更常见的情况：很多教学包已经把 RINEX 预处理成了 CSV 或 TXT，列名通常是GPS_Week, GPS_SOW, PRN, PseudoRange, CarrierPhase, C/N0, Elev, Azim。这种情况不用纠结 RINEX，直接用readmatrix读进工作区，先把列号对应起来。

2.2 用 MATLAB 解析 RINEX 头文件的骨架代码

如果包里确实是标准 RINEX，用一个最小解析函数就能把头文件里最关键的几项抠出来：

function hdr = parse_rinex_obs_header(fid) % 按行解析 RINEX 2.11 观测文件头，返回结构体 hdr = struct(); while true line = fgetl(fid); if ~ischar(line), break; end if contains(line, 'END OF HEADER'), break; end if contains(line, 'RINEX VERSION') hdr.version = str2double(line(1:9)); hdr.ftype = line(20); % O=观测, N=星历 elseif contains(line, 'APPROX POSITION XYZ') hdr.xyz = sscanf(line(1:42), '%lf')'; % 接收机近似坐标，3 个分量 elseif contains(line, 'TIME OF FIRST OBS') hdr.t0 = str2double(strsplit(strtrim(line(1:43)))); elseif contains(line, '# / TYPES OF OBSERV') hdr.nobs = str2double(line(1:6)); hdr.obstypes = strtrim(line(10:end)); end end end

调用的方式很简单：fid = fopen('obs_file.obs'); hdr = parse_rinex_obs_header(fid);。函数里line(1:9)是 RINEX 头文件规定的固定列位，sscanf负责处理连续数字。很多包作者喜欢把解析函数写成正则表达式版，但固定列位截取在 RINEX 2 系更稳，因为不同接收机的空格填充习惯不一样，正则反而容易漏匹配。

头文件解析完，观测数据部分就需要另一套循环了。这里最容易出问题的点是：一个历元里卫星数和头文件声明的nobs不一致，有的程序按nobs固定长度读，遇到卫星数变化直接错位。稳妥做法是按行尾的 PRN 编号循环，一个历元一个历元地收数据，而不是一整个矩阵读入。

2.3 从文件到工作区：对齐、单位、缺失值的 3 个坑

第一是时间系统。RINEX 2.11 头文件里的时间默认是 GPS Time，转 UTC 要扣闰秒；如果你手里的 CSV 是历元时间而星历用的是星期秒，两者错开哪怕 1 秒，卫星位置偏差就能到几百米。包里的程序如果自带gps2utc函数，优先相信并检查它扣的是不是整秒。

第二是伪距类型标识。C1C、C1P、P1代表不同的码，码间偏差一般有几十厘米到几米不等。如果观测文件混合了多个类型而程序没有区分，伪距残差会出现同一颗卫星同号偏置。

第三是 CSV 里的缺失值。readmatrix会把空格和空值都解析成NaN，但如果作者写的是0，程序会默认该卫星参与解算，然后解出一颗「零伪距」的荒谬结果。所以读入之后第一件事永远是画图：把每个 PRN 的伪距和 C/N0 各画一条曲线，看看有没有跳变。这个检查比任何封装好的读取函数都管用。

提示：数据质量没问题再继续往下走。伪距曲线异常直接查时间换算和单位换算，不用怀疑后面的解算算法。定位解算这个领域 80% 的错误出在数据预处理，而不是滤波本身。

3. 伪距单点定位解算的最小二乘实现与 AMP 加权策略

3.1 伪距观测方程的线性化

伪距观测方程写出来是：

ρ_i = ||r_sat,i - r_rec|| + c·δt + ε_i

四个未知数：接收机三维坐标和接收机钟差 δt，所以最少需要四颗星。方程里位置在范数里面，是非线性的，所以要在某个初始点泰勒展开，得到线性化的误差方程：

δz_i = h_i·δx + ε_i

δz是观测伪距与计算伪距的差，h_i是第 i 颗星到接收机的单位视线向量，δx是位置和钟差的增量。迭代流程是：初始位置给接收机近似坐标（RINEX 头里有），初始钟差给 0，算视线向量，解方程，更新坐标，直到增量小于阈值。常见做法是迭代 5~10 次，阈值 1e-4 米基本足够。线性化之后，整颗卫星的几何信息都集中在视线向量上，这也是后面算 DOP 值的基础。

3.2 迭代最小二乘的 MATLAB 实现

下面这段代码完成单历元伪距解算，核心是设计矩阵 H 和权矩阵 W 的构建：

function [pos, dtr, res, H, n_used] = ls_solve(sat_pos, rho, w, x0) % 最小二乘伪距单点定位 % sat_pos: 卫星位置 (n,3) ECEF % rho: 伪距观测值 (n,1) % w: 观测权向量 (n,1) % x0: 接收机近似坐标 (3,1) + 初始钟差 x = [x0; 0]; % 状态: x,y,z,cdt for k = 1:10 n = size(sat_pos, 1); H = zeros(n, 4); dz = zeros(n, 1); for i = 1:n dx = sat_pos(i,:) - x(1:3)'; r = norm(dx); H(i,1:3) = dx / r; % 视线方向单位向量 H(i,4) = 1; % 钟差列 dz(i) = rho(i) - r - x(4); % 残差计算 end W = diag(w); % 权矩阵，来自 AMP 或高度角模型 dx = (H'*W*H) \ (H'*W*dz); % 加权最小二乘 x = x + dx; if norm(dx) < 1e-4, break; end % 迭代收敛 end pos = x(1:3); dtr = x(4); res = dz - H*dx; % 最终残差 n_used = sum(w > 0); end

这里的 H 矩阵第四列是钟差系数，数学上等价于把所有伪距同时加同一个公共偏差。H'*W*H的维度是 4×4，对定位级计算来说求逆代价可以忽略。w的初值如果全是 1，就是普通最小二乘，精度差一些；实际包里会用高度角或 C/N0 生成非均匀权值，这就是 AMP 要做的事。迭代收敛条件用位置增量阈值，10 次上限对静态场景足够了，动态场景建议把上限提到 15 次。

3.3 AMP：把信噪比和高度角折算成观测权重

AMP 在这个标题里指的是自适应测量处理（Adaptive Measurement Processing），工程包里的函数名经常是amp_weight或者adaptive_obs_weight。它的输入是高度角和 C/N0，输出是每个观测量的方差或权重。三种最常见模型：

三种模型的核心思想都是让低质量观测自动降权。低高度角卫星穿过大气路径长，伪距噪声和多路径误差成倍放大；C/N0 直接反映信号质量，低于 30 dB-Hz 的观测量基本不能信。代码实现可以写成：

function sigma = amp_sigma(elev, cn0) % AMP 权重：高度角与 C/N0 联合方差模型 a = 0.3; b = 0.9; % 高度角项系数，单位米 c = 10^(30/10) * 0.1; % C/N0 门限归一化 el_sigma = sqrt(a^2 + b^2 / sin(elev).^2); cn_sigma = c * 10.^(-cn0/10); sigma = sqrt(el_sigma.^2 + cn_sigma.^2); end

参数a和b的物理含义是伪距噪声基底，基准站级的测量噪声可以给a=0.1, b=0.3，低成本接收机给大 3 倍。C/N0 项的常数决定了 40 dB-Hz 信号对应多少标准差，这个值不好拍脑袋定，常见做法是拿一段静态数据的残差拟合出来——先把残差画出来看长尾分布，再反推系数，一次就能对上。AMP 模块做得好不好，直接决定 EKF 里 R 矩阵可信不可信。

3.4 残差检查：第一次发现 GPS 误差的地方

解算完成后不要直接看坐标，先看残差向量。伪距残差如果整体不接近零均值，多半是钟差初值错了或者有某颗星伪距有偏。这时候有两个顺手操作：按 3σ 准则剔除残差超限的卫星，重解一遍；再把每颗星各历元的残差序列画出来，看是不是随时间缓慢变化的系统偏差——是的话，那是电离层或对流层残余误差，不是接收机噪声。

卫星几何强度也可以在这一步检查。H 矩阵算出来后，inv(H'*W*H)的前三个对角元开根号就是位置 DOP 分量。PDOP 大于 6 的时候，横纵 CDF 曲线会明显变肥，误差从米级跳到十几米都很正常。很多「GPS 误差」排查到最后，不是算法问题，是这一颗卫星的几何结构本来就不好。

4. 定位解算的动态滤波：从最小二乘到扩展卡尔曼滤波

4.1 动态场景下为什么单历元最小二乘不够用

最小二乘每个历元独立解算，没有把上一历元的位置信息带进来。接收机静止时，坐标会围绕真值来回抖；接收机运动时，方向一变化，误差直接被放大。EKF 的原理是状态预测加量测更新：状态方程负责把速度、钟说漂和上一历元的位置约束到一起，量测更新负责吸收当前历元的伪距信息。低成本 GPS 模块在树荫、高架下的连续定位体验，就是靠滤波撑住的。树莓派加 GPS 模块这类场景里，原始输出 5 Hz，LS 解出来位置噪声能到十几米，EKF 平滑后能收敛到 3~5 米，差距主要来自时间相关性的利用。

4.2 状态向量、转移矩阵与 Q 的约定

伪距定位的 EKF 状态向量一般取 8 维：位置三维、速度三维、接收机钟差、钟漂。位置和速度用常速度模型，钟差和钟漂用随机游走模型。转移矩阵 F 写成：

F = eye(8); dt = 1; % 历元间隔，秒 F(1,4) = dt; F(2,5) = dt; F(3,6) = dt; % 位置对速度 % 钟差与钟漂：F(7,8) = dt，如果估计两个状态 F(7,8) = dt;

过程噪声 Q 的经验值是按接收机动态等级划分的，这里给一个常用范围：

Q 设置太小会导致滤波收敛后偏置不更新，Q 太大会让滤波变成逐个历元的 LS，所以动态等级先定下，再调一两个数量级看效果。

4.3 EKF 预测-更新循环的 MATLAB 实现

function [x, P] = ekf_update(x, P, sat_pos, rho, w, F, Q) % EKF 预测-更新一步 % x: (8,1) 状态，前三维位置，4~6 速度，7 钟差，8 钟漂 % P: (8,8) 协方差 % F: (8,8) 状态转移矩阵，Q: (8,8) 过程噪声 % w: (n,1) AMP 权重，用于构建 R % 预测，常见做法是加 dt：P = F*P*F' + Q P = F * P * F' + Q; % 构建量测更新需要的 H 和 R n = size(sat_pos, 1); H = zeros(n, 8); R = diag(1 ./ w); % 权重转方差 dz = zeros(n, 1); for i = 1:n dx = sat_pos(i,:) - x(1:3)'; r = norm(dx); H(i,1:3) = dx / r; H(i,4:6) = 0; % 速度与伪距无关 H(i,7) = 1; % 钟差 H(i,8) = 0; % 钟漂与伪距测量无关 dz(i) = rho(i) - r - x(7); end % 卡尔曼增益与状态更新 S = H * P * H' + R; K = P * H' / S; x = x + K * dz; P = (eye(8) - K * H) * P; end

这里的 H 矩阵扩展成了 8 列，但实际只有位置和钟差列起作用。速度通过状态转移矩阵 F 影响下一历元的位置预测，不直接出现在观测方程里。伪距观测量对钟漂没有直接敏感度，这一点和载波相位解不同。处理多普勒观测时才需要给钟漂列加系数，如果包里有多普勒，把H(i,8)设为对多普勒的偏导数就行。

4.4 R 和 AMP 的关系：先让权重告诉系统误差

EKF 里最容易配错的是 R 矩阵。上一节 AMP 输出的方差直接就是 R 的对角元，如果 AMP 低估了伪距噪声，滤波会过度信任量测，位置曲线反而比 LS 还毛糙；高估了噪声，滤波响应变慢，转弯处会拉出弧线。判断依据是残差新息序列：innov = dz如果长期同号，说明 Q 偏小或 R 偏大；长期高频波动，说明 R 偏小。

另外，对低高度角卫星的观测，伪距误差并不是严格高斯的，粗差比例不低。EKF 在量测更新前可以先跑一遍卡方检验：

nu = length(dz); if dz' / S * dz > chi2inv(0.99, nu) % 粗差超限，本历元量测更新降级为纯预测 x = x; P = P; % 等价于跳过更新 end

这个阈值根据 2 自由度卡方分布查表，chi2inv(0.99, 4)约等于 13.28。城市峡谷里粗差频繁时，把阈值放宽到 0.999 对应的值，避免一整个历元都不更新。

5. 定位解算结果的验证与关键参数调参方法

坐标算出来不是终点，验证这一步能帮你判断「程序跑通」和「程序跑对」之间的差距。最直接的做法是让接收机静止在已知点上，把解算坐标与真值比较，统计三个方向的误差 CDF。下面这段代码输出 95% 误差分位数：

% err: 定位误差序列，单位米，列方向为 1 [p, err_q] = ecdf(err); p95 = err_q(find(p > 0.95, 1)); figure; plot(err_q, p*100); ylabel('CDF (%)'); exportgraphics(gcf, 'err_cdf.eps'); % 论文插图直接导出 eps

这个exportgraphics在 2020a 之后的 MATLAB 版本通用，比老式print -depsc对中文字体兼容好，不用切画布大小就能直接嵌入 LaTeX 论文。

第二个关键技巧是用单位权重方差检查 R 矩阵是否缩放正确。验后单位权重方差定义为σ0² = (残差^T · W · 残差)/(n - u)，其中 u 是 4（LS）或 8（EKF）。如果 σ0 明显大于 1，说明你给的伪距方差整体偏小，R 要整体放大 σ0² 倍；如果小于 1 则是方差偏大。这个数字是个标量，改起来非常快，比逐颗星调权重的效率高一个数量级。做完这一步，再回头看 CDF 曲线，95% 分位数才是你真正可以写到报告里的精度值。

最后说一个调参顺序：先调 AMP 的系数把残差压到接近正态，再动 Q，最后才动 R 的整体缩放。顺序反了，你会陷入「参数调哪都对，合起来就错」的泥潭。定位解算程序的代码量不大，但参数之间的耦合关系比代码结构复杂得多，顺着这个顺序来，最能省时间。

本文还有配套的精品资源，点击获取