基于最小二乘法的五颗可见卫星伪距定位

一、数学模型构建

1.1 伪距观测方程

对于每颗可见卫星i，观测方程可表示为：

( $x,y,z$ )：接收机三维坐标（待解算）
( $x_i,y_i,z_i)$ ：卫星 $i$ 的ECEF坐标（由星历计算）
$Δt$ ：接收机钟差（待解算）
$ϵi$ ：测量噪声（含电离层、多径等误差）

1.2 线性化处理

在初始估计点( $x0,y0,z0,Δt0$ )处进行泰勒展开，保留一阶项：

其中：

1.3 设计矩阵构建

构建5×4设计矩阵A：

$d_i$ ：接收机到卫星i的初始几何距离
每行对应一颗卫星的观测方程

二、解算流程

2.1 初始值估计

使用前4颗卫星通过闭式解计算初始位置和钟差：

2.2 迭代优化

步骤1：计算残差 $δρi=ρi−ρ^i$

步骤2：构建增广矩阵 $[A^T∣b]$

步骤3：求解正规方程：

$(A_TA)Δx=A_Tb$

步骤4：更新参数：

$x_{k+1}=x_k+(A^TA)^{−1}A^Tb$

收敛条件： $∥Δx∥<10^{−6}（$ 或迭代次数>10次）

三、MATLAB实现代码

%% 五颗卫星伪距定位
% 输入参数
sat_pos = [12345678.9, 23456789.1, 34567890.2;  % 卫星1坐标 (ECEF)
          12345679.0, 23456789.2, 34567890.3;  % 卫星2
          12345679.1, 23456789.3, 34567890.4;  % 卫星3
          12345679.2, 23456789.4, 34567890.5;  % 卫星4
          12345679.3, 23456789.5, 34567890.6]; % 卫星5
rho = [22222222.2; 22222223.3; 22222224.4; 22222225.5; 22222226.6]; % 伪距观测值

% 初始估计（使用前4颗卫星）
x0 = 12345678.0; y0 = 23456789.0; z0 = 34567890.0; dt0 = 0;
for i = 1:4
    dx = (x0 - sat_pos(i,1)) / norm(sat_pos(i,:) - [x0,y0,z0]);
    dy = (y0 - sat_pos(i,2)) / norm(sat_pos(i,:) - [x0,y0,z0]);
    dz = (z0 - sat_pos(i,3)) / norm(sat_pos(i,:) - [x0,y0,z0]);
    dt0 = dt0 + (rho(i) - norm(sat_pos(i,:) - [x0,y0,z0])) / c;
end
dt0 = dt0 / 4;

% 构建设计矩阵A和观测向量b
A = zeros(5,4);
b = zeros(5,1);
for i = 1:5
    d0 = norm(sat_pos(i,:) - [x0,y0,z0]);
    A(i,:) = [ (x0 - sat_pos(i,1))/d0, (y0 - sat_pos(i,2))/d0, (z0 - sat_pos(i,3))/d0, c ];
    b(i) = rho(i) - d0;
end

% 加权最小二乘解算（假设各星方差相同）
W = eye(5); % 等权重
dx = (A' * W * A) \ (A' * W * b);
x = [x0; y0; z0; dt0] + dx;

% 输出结果
fprintf("位置误差: %.3f m\n", norm(dx(1:3)));
fprintf("钟差误差: %.1f ns\n", dx(4)*1e9);

四、精度评估与优化

4.1 误差分析

误差源	影响量级	补偿方法
电离层延迟	5-50 m	Klobuchar模型
对流层延迟	2-20 m	Saastamoinen模型
多径效应	1-10 m	双频组合观测
接收机钟差	10-100 ns	超紧耦合技术

4.2 性能提升策略

加权最小二乘：根据卫星高度角分配权重（高仰角卫星权重更大）

% 高度角加权矩阵
elevation = [45, 30, 60, 25, 40](@ref); % 卫星高度角(度)
W = diag(1./(elevation.^2)); % 权重与高度角平方成反比

抗差估计：采用Huber损失函数抑制异常值影响

function [delta] = huber_update(rho, rho_pred, delta_prev, c=1.345)
    residuals = rho - rho_pred;
    weights = 1 ./ max(1, abs(residuals)/c);
    delta = (A' * diag(weights) * A) \ (A' * diag(weights) * residuals);
end

动态解算：结合INS辅助提高动态场景精度

五、参考

刘经南, 张小红. 全球定位系统原理与数据处理[M]. 武汉大学出版社, 2022.
代码采用最小二乘法对有5颗可见卫星的情况下对接收机进行了定位 www.youwenfan.com/contentcnl/79963.html
陈俊平, 王解先. GNSS精密定位理论与方法[M]. 测绘出版社, 2021.
伪距单点定位DOP值分析.