伪距单点定位的MATLAB实现方法

伪距单点定位(Single Point Positioning, SPP)是利用GPS卫星发射的伪距观测值确定接收机位置的基本方法。

MATLAB代码实现

% 伪距单点定位MATLAB实现

clear; clc; close all;

%% 参数设置
c = 299792458;       % 光速 (m/s)
omega_e = 7.292115e-5; % 地球自转角速度 (rad/s)
mu = 3.986004418e14;  % 地球引力常数 (m^3/s^2)

% 初始位置估计 (WGS84椭球)
init_pos = [0, 0, 6378137]; % [纬度, 经度, 高度] (度,度,m)

% 接收机钟差初始估计 (秒)
init_clk_bias = 0;

% 迭代参数
max_iter = 10;       % 最大迭代次数
tol = 1e-4;          % 收敛阈值 (米)

%% 读取RINEX导航文件
nav_file = 'brdc0010.21n'; % 替换为实际文件路径
[ephemeris, iono_params, leap_seconds] = read_rinex_nav(nav_file);

%% 读取RINEX观测文件
obs_file = 'rinex0010.21o'; % 替换为实际文件路径
[obs_data, obs_time] = read_rinex_obs(obs_file);

%% 选择特定时刻的观测数据
target_time = obs_time(1); % 使用第一个历元
pr_obs = [];              % 存储伪距观测值
sat_prns = [];            % 存储卫星PRN号
sat_elev = [];            % 存储卫星高度角

% 筛选GPS卫星 (PRN 1-32)
for sat_id = 1:length(obs_data)
    prn = obs_data(sat_id).prn;
    if prn >= 1 && prn <= 32
        if isfield(obs_data(sat_id).obs, 'C1C')
            pr_obs(end+1) = obs_data(sat_id).obs.C1C(target_time);
            sat_prns(end+1) = prn;
        end
    end
end

fprintf('使用 %d 颗GPS卫星进行定位\n', length(pr_obs));

%% 主定位循环
pos_est = init_pos;       % 初始位置估计
clk_bias = init_clk_bias; % 初始钟差估计

for iter = 1:max_iter
    % 计算卫星位置
    [sat_pos, sat_clk] = compute_sat_positions(ephemeris, target_time, sat_prns);
    
    % 计算几何距离
    geo_dist = sqrt(sum((sat_pos - repmat(pos_est, size(sat_pos,1), 1)).^2, 2));
    
    % 构建观测方程
    A = zeros(length(pr_obs), 4);
    b = zeros(length(pr_obs), 1);
    
    for i = 1:length(pr_obs)
        % 卫星到接收机的单位矢量
        los_vector = (sat_pos(i,:) - pos_est) / geo_dist(i);
        
        % 设计矩阵A
        A(i, 1:3) = los_vector;
        A(i, 4) = 1; % 接收机钟差项
        
        % 常数项b (伪距残差)
        b(i) = pr_obs(i) - (geo_dist(i) + c*(sat_clk(i) - clk_bias));
    end
    
    % 最小二乘解算
    delta_x = pinv(A) * b;
    
    % 更新估计值
    pos_est = pos_est + delta_x(1:3)';
    clk_bias = clk_bias + delta_x(4)/c; % 转换为秒
    
    % 检查收敛
    if norm(delta_x(1:3)) < tol
        fprintf('迭代 %d: 收敛于 %.4f 米\n', iter, norm(delta_x(1:3)));
        break;
    elseif iter == max_iter
        fprintf('达到最大迭代次数 %d, 残差 %.4f 米\n', max_iter, norm(delta_x(1:3)));
    end
end

%% 转换结果为经纬高
lla_est = ecef2lla(pos_est);
fprintf('\n定位结果:\n');
fprintf('ECEF坐标: X=%.3f m, Y=%.3f m, Z=%.3f m\n', pos_est);
fprintf('经纬高: 纬度=%.6f°, 经度=%.6f°, 高程=%.3f m\n', lla_est(1), lla_est(2), lla_est(3));
fprintf('接收机钟差: %.9f 秒 (约 %.3f ns)\n', clk_bias, clk_bias*1e9);

%% 可视化结果
figure;
geoscatter(lla_est(1), lla_est(2), 100, 'r', 'filled');
hold on;
geobasemap('streets');
title('伪距单点定位结果');
text(lla_est(2), lla_est(1), sprintf('定位点\n%.6f°N, %.6f°E', lla_est(1), lla_est(2)), ...
    'VerticalAlignment', 'bottom');

%% 辅助函数: 读取RINEX导航文件
function [ephemeris, iono_params, leap_seconds] = read_rinex_nav(filename)
    % 简化的RINEX导航文件读取函数
    % 实际应用中应使用完整的解析器
    
    ephemeris = struct();
    iono_params = zeros(8,1); % 8个电离层参数
    leap_seconds = 18; % 默认值
    
    fid = fopen(filename, 'r');
    if fid == -1
        error('无法打开导航文件: %s', filename);
    end
    
    % 跳过文件头
    while true
        line = fgetl(fid);
        if contains(line, 'END OF HEADER')
            break;
        end
        
        % 读取电离层参数
        if contains(line, 'ION ALPHA')
            alpha_str = textscan(line(4:end), '%f %f %f %f');
            iono_params(1:4) = alpha_str{1};
        end
        if contains(line, 'ION BETA')
            beta_str = textscan(line(4:end), '%f %f %f %f');
            iono_params(5:8) = beta_str{1};
        end
    end
    
    % 读取星历数据
    sat_count = 0;
    while ~feof(fid)
        line1 = fgetl(fid);
        if isempty(line1)
            continue;
        end
        
        % 解析第一行
        sat_prn = str2double(line1(1:2));
        epoch_str = line1(4:23);
        year = str2double(line1(4:5)) + 2000;
        month = str2double(line1(7:8));
        day = str2double(line1(10:11));
        hour = str2double(line1(13:14));
        minute = str2double(line1(16:17));
        second = str2double(line1(19:22));
        epoch = datetime(year, month, day, hour, minute, second);
        
        % 读取后续行
        line2 = fgetl(fid);
        line3 = fgetl(fid);
        line4 = fgetl(fid);
        line5 = fgetl(fid);
        
        % 解析星历参数
        data = sscanf([line1(24:end) line2 line3 line4 line5], '%f');
        
        % 存储星历
        sat_count = sat_count + 1;
        ephemeris(sat_count).prn = sat_prn;
        ephemeris(sat_count).epoch = epoch;
        ephemeris(sat_count).sqrtA = data(11);
        ephemeris(sat_count).e = data(8);
        ephemeris(sat_count).i0 = deg2rad(data(17));
        ephemeris(sat_count).Omega0 = deg2rad(data(15));
        ephemeris(sat_count).omega = deg2rad(data(19));
        ephemeris(sat_count).M0 = deg2rad(data(16));
        ephemeris(sat_count).delta_n = data(12);
        ephemeris(sat_count).Omega_dot = deg2rad(data(21));
        ephemeris(sat_count).i_dot = deg2rad(data(23));
        ephemeris(sat_count).Cuc = data(4);
        ephemeris(sat_count).Cus = data(5);
        ephemeris(sat_count).Crc = data(6);
        ephemeris(sat_count).Crs = data(7);
        ephemeris(sat_count).Cic = data(2);
        ephemeris(sat_count).Cis = data(3);
        ephemeris(sat_count).a_f0 = data(24);
        ephemeris(sat_count).a_f1 = data(25);
        ephemeris(sat_count).a_f2 = data(26);
    end
    
    fclose(fid);
    fprintf('成功读取 %d 颗卫星的星历数据\n', sat_count);
end

%% 辅助函数: 读取RINEX观测文件
function [obs_data, obs_times] = read_rinex_obs(filename)
    % 简化的RINEX观测文件读取函数
    
    obs_data = struct();
    obs_times = [];
    
    fid = fopen(filename, 'r');
    if fid == -1
        error('无法打开观测文件: %s', filename);
    end
    
    % 跳过文件头
    while true
        line = fgetl(fid);
        if contains(line, 'END OF HEADER')
            break;
        end
        
        % 读取时间信息
        if contains(line, 'TIME OF FIRST OBS')
            parts = strsplit(line);
            year = str2double(parts{1});
            month = str2double(parts{2});
            day = str2double(parts{3});
            hour = str2double(parts{4});
            minute = str2double(parts{5});
            second = str2double(parts{6});
            first_epoch = datetime(year, month, day, hour, minute, second);
        end
    end
    
    % 读取观测数据
    sat_index = 0;
    while ~feof(fid)
        % 读取历元行
        epoch_line = fgetl(fid);
        if isempty(epoch_line) || epoch_line(1) == ')'
            continue;
        end
        
        % 解析历元时间
        year = str2double(epoch_line(1:4));
        month = str2double(epoch_line(6:7));
        day = str2double(epoch_line(9:10));
        hour = str2double(epoch_line(12:13));
        minute = str2double(epoch_line(15:16));
        second = str2double(epoch_line(18:19));
        epoch = datetime(year, month, day, hour, minute, second);
        obs_times(end+1) = epoch;
        
        % 读取卫星数量
        num_sats = str2double(epoch_line(31:32));
        
        % 读取卫星列表
        sat_list = zeros(num_sats, 1);
        for s = 1:ceil(num_sats/12)
            line = fgetl(fid);
            for k = 1:min(12, num_sats - (s-1)*12)
                col_start = 4 + (k-1)*3;
                sat_list((s-1)*12 + k) = str2double(line(col_start:col_start+2));
            end
        end
        
        % 读取观测值
        for s = 1:num_sats
            sat_id = sat_list(s);
            line1 = fgetl(fid);
            line2 = fgetl(fid);
            line3 = fgetl(fid);
            line4 = fgetl(fid);
            
            % 解析观测值
            obs_values = [];
            if ~isempty(line1)
                values = sscanf(line1(6:end), '%f');
                obs_values = [obs_values; values];
            end
            if ~isempty(line2)
                values = sscanf(line2(6:end), '%f');
                obs_values = [obs_values; values];
            end
            if ~isempty(line3)
                values = sscanf(line3(6:end), '%f');
                obs_values = [obs_values; values];
            end
            if ~isempty(line4)
                values = sscanf(line4(6:end), '%f');
                obs_values = [obs_values; values];
            end
            
            % 存储观测数据
            if ~isfield(obs_data, ['sat_' num2str(sat_id)])
                sat_index = sat_index + 1;
                obs_data(sat_index).prn = sat_id;
                obs_data(sat_index).obs = struct();
            else
                for k = 1:length(obs_data)
                    if obs_data(k).prn == sat_id
                        sat_index = k;
                        break;
                    end
                end
            end
            
            % 假设前几个观测值是C1C, L1C, D1C, S1C等
            if length(obs_values) >= 1
                obs_data(sat_index).obs.C1C = obs_values(1);
            end
            if length(obs_values) >= 2
                obs_data(sat_index).obs.L1C = obs_values(2);
            end
            if length(obs_values) >= 3
                obs_data(sat_index).obs.D1C = obs_values(3);
            end
            if length(obs_values) >= 4
                obs_data(sat_index).obs.S1C = obs_values(4);
            end
        end
    end
    
    fclose(fid);
    fprintf('成功读取 %d 个历元的观测数据\n', length(obs_times));
end

%% 辅助函数: 计算卫星位置
function [sat_pos, sat_clk] = compute_sat_positions(ephemeris, epoch, prns)
    % 计算指定时刻卫星位置和钟差
    
    num_sats = length(prns);
    sat_pos = zeros(num_sats, 3);
    sat_clk = zeros(num_sats, 1);
    
    for i = 1:num_sats
        prn = prns(i);
        sat_ephem = [];
        
        % 查找对应PRN的星历
        for k = 1:length(ephemeris)
            if ephemeris(k).prn == prn
                sat_ephem = ephemeris(k);
                break;
            end
        end
        
        if isempty(sat_ephem)
            warning('未找到PRN %d的星历数据', prn);
            continue;
        end
        
        % 计算卫星钟差
        dt = hours(epoch - sat_ephem.epoch) + minutes(epoch - sat_ephem.epoch) + seconds(epoch - sat_ephem.epoch);
        sat_clk(i) = sat_ephem.a_f0 + sat_ephem.a_f1*dt + sat_ephem.a_f2*dt^2;
        
        % 计算平近点角
        n0 = sqrt(mu / sat_ephem.sqrtA^6); % 平均运动
        tk = hours(epoch - sat_ephem.epoch) + minutes(epoch - sat_ephem.epoch) + seconds(epoch - sat_ephem.epoch);
        n = n0 + sat_ephem.delta_n;
        Mk = sat_ephem.M0 + n*tk;
        
        % 计算偏近点角 (开普勒方程迭代解)
        Ek = Mk;
        for j = 1:10
            Ek_prev = Ek;
            Ek = Mk + sat_ephem.e * sin(Ek_prev);
            if abs(Ek - Ek_prev) < 1e-12
                break;
            end
        end
        
        % 计算真近点角
        nu_k = atan2(sqrt(1-sat_ephem.e^2)*sin(Ek), cos(Ek)-sat_ephem.e);
        
        % 计算升交距角
        phi_k = nu_k + sat_ephem.omega;
        
        % 计算摄动改正
        du_k = sat_ephem.Cuc*cos(2*phi_k) + sat_ephem.Cus*sin(2*phi_k);
        dr_k = sat_ephem.Crc*cos(2*phi_k) + sat_ephem.Crs*sin(2*phi_k);
        di_k = sat_ephem.Cic*cos(2*phi_k) + sat_ephem.Cis*sin(2*phi_k);
        
        % 改正后的参数
        u_k = phi_k + du_k;
        r_k = sat_ephem.sqrtA^2*(1-sat_ephem.e*cos(Ek)) + dr_k;
        i_k = sat_ephem.i0 + sat_ephem.i_dot*tk + di_k;
        
        % 计算卫星位置 (轨道平面坐标系)
        x_k_prime = r_k * cos(u_k);
        y_k_prime = r_k * sin(u_k);
        
        % 计算升交点赤经
        Omega_k = sat_ephem.Omega0 + (sat_ephem.Omega_dot - omega_e)*tk - omega_e*sat_ephem.toe;
        
        % 计算地固坐标系中的位置
        sat_pos(i,1) = x_k_prime*cos(Omega_k) - y_k_prime*cos(i_k)*sin(Omega_k);
        sat_pos(i,2) = x_k_prime*sin(Omega_k) + y_k_prime*cos(i_k)*cos(Omega_k);
        sat_pos(i,3) = y_k_prime*sin(i_k);
        
        % 地球自转改正
        dt_rel = norm(sat_pos(i,:))/c;
        sat_pos(i,:) = rotate_ecef(sat_pos(i,:), omega_e*dt_rel);
    end
end

%% 辅助函数: ECEF旋转
function rotated_pos = rotate_ecef(pos, angle)
    % 绕Z轴旋转ECEF坐标
    rotation_matrix = [cos(angle), -sin(angle), 0;
                      sin(angle),  cos(angle), 0;
                      0,          0,          1];
    rotated_pos = rotation_matrix * pos';
    rotated_pos = rotated_pos';
end

%% 辅助函数: ECEF转经纬高
function lla = ecef2lla(ecef)
    % WGS84椭球参数
    a = 6378137; % 长半轴 (m)
    f = 1/298.257223563; % 扁率
    b = a*(1-f); % 短半轴
    e = sqrt((a^2 - b^2)/a^2); % 第一偏心率
    
    x = ecef(1);
    y = ecef(2);
    z = ecef(3);
    
    % 计算经度
    lon = atan2(y, x);
    
    % 迭代计算纬度和高度
    p = sqrt(x^2 + y^2);
    lat = atan2(z, p*(1-e^2));
    h = 0;
    
    for iter = 1:10
        N = a / sqrt(1 - e^2*sin(lat)^2);
        h_prev = h;
        h = p/cos(lat) - N;
        lat_prev = lat;
        lat = atan2(z, p*(1 - e^2*N/(N+h)));
        
        if abs(lat - lat_prev) < 1e-9 && abs(h - h_prev) < 1e-9
            break;
        end
    end
    
    % 转换为角度
    lat_deg = rad2deg(lat);
    lon_deg = rad2deg(lon);
    
    lla = [lat_deg, lon_deg, h];
end

算法原理详解

1. 伪距观测方程

伪距单点定位的核心方程是：

$\rho_i = \sqrt{(x_i - x_r)^2 + (y_i - y_r)^2 + (z_i - z_r)^2} + c \cdot \delta t_r - c \cdot \delta t_i + I_i + T_i + \varepsilon_i$

其中：

• $\rho_i$：接收机测量的伪距
• $(x_i, y_i, z_i)$：卫星位置
• $(x_r, y_r, z_r)$：接收机位置（未知）
• $c$：光速
• $\delta t_r$：接收机钟差（未知）
• $\delta t_i$：卫星钟差（已知或可计算）
• $I_i$：电离层延迟
• $T_i$：对流层延迟
• $\varepsilon_i$：多路径效应和噪声

简化后（忽略次要误差）：
$\rho_i \approx \sqrt{(x_i - x_r)^2 + (y_i - y_r)^2 + (z_i - z_r)^2} + c \cdot \delta t_r$

2. 线性化处理

将非线性观测方程在近似位置 $(x_{r0}, y_{r0}, z_{r0})$ 处线性化：

$\rho_i \approx \rho_{0i} + \frac{\partial \rho_i}{\partial x_r} \Delta x + \frac{\partial \rho_i}{\partial y_r} \Delta y + \frac{\partial \rho_i}{\partial z_r} \Delta z + c \cdot \Delta \delta t_r$

其中：

• $\rho_{0i} = \sqrt{(x_i - x_{r0})^2 + (y_i - y_{r0})^2 + (z_i - z_{r0})^2}$
• $\frac{\partial \rho_i}{\partial x_r} = \frac{x_{r0} - x_i}{\rho_{0i}}$（其他分量类似）

3. 最小二乘解算

将线性化方程写成矩阵形式：
$A \Delta X = b$

其中：

• $A$ 是设计矩阵（每行包含方向余弦和1）
• $\Delta X = [\Delta x, \Delta y, \Delta z, c \cdot \Delta \delta t_r]^T$
• $b$ 是伪距残差向量

解为：
$\Delta X = (A^T A)^{-1} A^T b$

4. 迭代过程

1. 初始位置估计（如[0,0,6378137] m）
2. 计算卫星位置
3. 构建并求解线性化方程
4. 更新位置估计
5. 重复2-4步直到收敛

数据准备与处理

1. RINEX文件格式

• 导航文件(.n)：包含卫星星历和时钟参数
• 观测文件(.o)：包含接收机测量的伪距

2. 关键数据处理步骤

• 解析RINEX文件头
• 提取星历参数（开普勒轨道根数）
• 计算卫星位置（考虑地球自转）
• 提取伪距观测值（C1C码）
• 处理时间系统转换

误差来源与改进措施

主要误差源

1. 卫星钟差：通过导航电文校正
2. 电离层延迟：双频接收机或模型校正
3. 对流层延迟：模型校正（Hopfield/Saastamoinen）
4. 多路径效应：选择开阔场地
5. 接收机噪声：高精度接收机

改进SPP精度的技术

1. 精密单点定位(PPP)：使用精密星历和钟差产品
2. 差分GPS(DGPS)：使用参考站校正
3. 多系统融合：GPS+GLONASS+Galileo+BDS
4. 卡尔曼滤波：动态模型平滑位置解

参考代码 伪距单点定位的matlab实现方法 www.youwenfan.com/contentcnr/100952.html

结果分析与可视化

输出结果

• ECEF坐标（地心地固坐标系）
• 经纬高（WGS84坐标系）
• 接收机钟差（纳秒级）

可视化方法

• 地理散点图显示定位点
• 收敛曲线显示迭代过程
• 卫星几何分布图（DOP值）

实际应用注意事项

1. 数据质量：使用高质量观测数据
2. 可见卫星数：至少需要4颗卫星
3. 几何分布：卫星高度角>15°
4. 时间同步：确保接收机时间与GPS时一致
5. 异常处理：检测周跳和粗差