尧图网页设计 
简介面向GPS定位初学者的AMP MATLAB定位解算程序围绕卫星导航中的几何定位问题提供从数据读取到结果输出的完整示例代码帮助用户理解伪距观测、卫星位置解算与最小二乘定位的基本逻辑。压缩包内共有五个文件包括三个MATLAB脚本分别负责单点位置解算、卫星位置速度计算、大地坐标转换和两个文本格式数据文件包含观测值文件和星历数据文件压缩包整体大小为11KB。目前已有458人学习下载适合测绘、导航或通信方向的本科生、研究生入门参考。代码结构和变量命名清晰关键步骤均附有注释从观测文件读取、误差修正到坐标输出各阶段均有对应函数实现涵盖数据预处理、电离层与对流层延迟校正、钟差估算、几何距离解算以及非线性最小二乘平差借助观测值与星历数据可完整复现定位流程也能在此基础上替换或增加函数模块扩展至多星座联合定位或RTK研究在提升编程能力的同时加深对卫星定位误差源和坐标转换细节的理解。1. 解压完「卫星定位解算数据程序.rar」之后先确认手里有什么解压完这个 rar多数人第一件事是找 main.m然后直接 F5。这个标题其实在描述一类很典型的交付物一批 GPS 原始观测数据加上一套用 MATLAB 写的定位解算程序。AMP 在函数命名里出现时通常是「自适应测量处理」的缩写负责把信号质量转换成定位解的权重不是 Google 那个 AMP这一点先别理解偏。这篇文章会先把 RINEX 数据读进 MATLAB用最小二乘把伪距定位解算跑通再把 EKF 的 Q/R 配起来最后落在输出坐标的验证方法上。适合课程设计、开题前的算法复现也适合第一次拿到陌生定位代码包的工程师——你不需要猜作者把数据藏在了哪一列按下面这套流程顺下来就能对上号。2. 卫星定位解算的第一步把观测数据读进 MATLAB2.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输出是每个观测量的方差或权重。三种最常见模型模型公式适用场景高度角正弦σ² a² b² / sin²(el)通用静态/动态忽略信号强度C/N0 指数σ² C1·10^(-C/N0/10)城市峡谷、多路径严重环境高度角CN0 联合两个方差相加低成本接收机树底下漂移大三种模型的核心思想都是让低质量观测自动降权。低高度角卫星穿过大气路径长伪距噪声和多路径误差成倍放大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的物理含义是伪距噪声基底基准站级的测量噪声可以给a0.1, b0.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 HzLS 解出来位置噪声能到十几米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 的经验值是按接收机动态等级划分的这里给一个常用范围接收机动态速度过程噪声 Qv (m²)钟漂过程噪声 Qc (m²/s²)静态1e-41e-4步行/骑行0.011e-3车载0.1~11e-3机载10~1001e-3Q 设置太小会导致滤波收敛后偏置不更新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 % 预测常见做法是加 dtP 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 是 4LS或 8EKF。如果 σ0 明显大于 1说明你给的伪距方差整体偏小R 要整体放大 σ0² 倍如果小于 1 则是方差偏大。这个数字是个标量改起来非常快比逐颗星调权重的效率高一个数量级。做完这一步再回头看 CDF 曲线95% 分位数才是你真正可以写到报告里的精度值。最后说一个调参顺序先调 AMP 的系数把残差压到接近正态再动 Q最后才动 R 的整体缩放。顺序反了你会陷入「参数调哪都对合起来就错」的泥潭。定位解算程序的代码量不大但参数之间的耦合关系比代码结构复杂得多顺着这个顺序来最能省时间。本文还有配套的精品资源点击获取 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. 卫星定位解算的第一步把观测数据读进 MATLAB2.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输出是每个观测量的方差或权重。三种最常见模型模型公式适用场景高度角正弦σ² a² b² / sin²(el)通用静态/动态忽略信号强度C/N0 指数σ² C1·10^(-C/N0/10)城市峡谷、多路径严重环境高度角CN0 联合两个方差相加低成本接收机树底下漂移大三种模型的核心思想都是让低质量观测自动降权。低高度角卫星穿过大气路径长伪距噪声和多路径误差成倍放大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的物理含义是伪距噪声基底基准站级的测量噪声可以给a0.1, b0.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 HzLS 解出来位置噪声能到十几米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 的经验值是按接收机动态等级划分的这里给一个常用范围接收机动态速度过程噪声 Qv (m²)钟漂过程噪声 Qc (m²/s²)静态1e-41e-4步行/骑行0.011e-3车载0.1~11e-3机载10~1001e-3Q 设置太小会导致滤波收敛后偏置不更新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 % 预测常见做法是加 dtP 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 是 4LS或 8EKF。如果 σ0 明显大于 1说明你给的伪距方差整体偏小R 要整体放大 σ0² 倍如果小于 1 则是方差偏大。这个数字是个标量改起来非常快比逐颗星调权重的效率高一个数量级。做完这一步再回头看 CDF 曲线95% 分位数才是你真正可以写到报告里的精度值。最后说一个调参顺序先调 AMP 的系数把残差压到接近正态再动 Q最后才动 R 的整体缩放。顺序反了你会陷入「参数调哪都对合起来就错」的泥潭。定位解算程序的代码量不大但参数之间的耦合关系比代码结构复杂得多顺着这个顺序来最能省时间。本文还有配套的精品资源点击获取