当前位置：首页 > news >正文

GPS与北斗组合单点定位算法MATLAB实现

news 2025/10/12 11:35:47

组合导航定位概述

系统特点对比

系统	星座数量	轨道类型	服务区域	频点特性
GPS	24+颗	MEO	全球	L1(1575.42MHz), L2(1227.60MHz)
北斗	35颗	GEO/IGSO/MEO	全球+区域增强	B1(1561.098MHz), B2(1207.140MHz)

组合定位优势

可见卫星数量增加：提高定位可用性和可靠性
几何构型改善：降低DOP值，提高定位精度
冗余备份：单系统故障时仍可定位
精度提升：更多观测值改善解算结果

数学模型与算法原理

1. 伪距观测方程

GPS伪距观测方程：
$PG,i=ρG,i+c(δtr−δtG,i)+IG,i+TG,i+εG,iP_{G,i} = \rho_{G,i} + c(\delta t_r - \delta t_{G,i}) + I_{G,i} + T_{G,i} + \varepsilon_{G,i}$

北斗伪距观测方程：
$PB,j=ρB,j+c(δtr−δtB,j)+δtsys+IB,j+TB,j+εB,jP_{B,j} = \rho_{B,j} + c(\delta t_r - \delta t_{B,j}) + \delta t_{sys} + I_{B,j} + T_{B,j} + \varepsilon_{B,j}$

其中：

$P$ ：伪距观测值
$ρ\rho$ ：几何距离
$c$ ：光速
$δtr\delta t_r$ ：接收机钟差
$δtG,i\delta t_{G,i}$ 、 $δtB,j\delta t_{B,j}$ ：卫星钟差
$δtsys\delta t_{sys}$ ：系统间时间偏差
$I$ ：电离层延迟
$T$ ：对流层延迟
$ε\varepsilon$ ：观测噪声

2. 线性化观测方程

将观测方程在近似坐标处线性化：

$ΔP=H⋅ΔX+ε\Delta P = H \cdot \Delta X + \varepsilon$

其中：

$ΔP\Delta P$ ：伪距残差向量
$H$ ：设计矩阵
$ΔX\Delta X$ ：待估参数向量 $[Δx,Δy,Δz,cδtr,cδtsys]T[\Delta x, \Delta y, \Delta z, c\delta t_r, c\delta t_{sys}]^T$

实现

1. 主程序 - 组合单点定位

function [position, covariance, residuals, dop] = gps_bds_spp(obs_data, ephem_gps, ephem_bds, init_pos)% GPS与北斗组合单点定位% 输入：%   obs_data: 观测数据结构体%   ephem_gps: GPS星历数据%   ephem_bds: 北斗星历数据%   init_pos: 初始位置估计 [x; y; z] (ECEF, m)% 输出：%   position: 定位结果 [x; y; z] (ECEF, m)%   covariance: 协方差矩阵%   residuals: 残差向量%   dop: 精度因子结构体% 参数设置max_iterations = 20;convergence_threshold = 0.01; % 收敛阈值 (m)weight_matrix = []; % 权重矩阵% 初始化if nargin < 4 || isempty(init_pos)init_pos = [0; 0; 0]; % 默认初始位置endcurrent_pos = init_pos;receiver_clock_error = 0; % 接收机钟差 (m)system_time_offset = 0;   % 系统间时间偏差 (m)fprintf('开始GPS/北斗组合单点定位...\n');for iter = 1:max_iterations% 构建观测方程[H, delta_rho, sat_positions, sat_ids, system_flags] = ...build_observation_equation(current_pos, receiver_clock_error, ...system_time_offset, obs_data, ...ephem_gps, ephem_bds);if size(H, 1) < 4error('可见卫星数量不足，至少需要4颗卫星');end% 构建权重矩阵（基于卫星高度角）if isempty(weight_matrix)weight_matrix = calculate_weight_matrix(sat_positions, current_pos, system_flags);end% 最小二乘解算H_weighted = weight_matrix * H;delta_rho_weighted = weight_matrix * delta_rho;% 正则化处理（防止病态矩阵）delta_X = (H_weighted' * H_weighted + 1e-6 * eye(size(H,2))) \ (H_weighted' * delta_rho_weighted);% 更新参数current_pos = current_pos + delta_X(1:3);receiver_clock_error = receiver_clock_error + delta_X(4);if length(delta_X) > 4system_time_offset = system_time_offset + delta_X(5);end% 检查收敛position_change = norm(delta_X(1:3));fprintf('迭代 %d: 位置变化 = %.4f m\n', iter, position_change);if position_change < convergence_thresholdfprintf('收敛于迭代 %d\n', iter);break;endif iter == max_iterationswarning('达到最大迭代次数，可能未完全收敛');endend% 计算最终结果position = current_pos;% 计算协方差矩阵covariance = calculate_covariance_matrix(H, weight_matrix, delta_rho);% 计算残差residuals = delta_rho - H * delta_X;% 计算精度因子dop = calculate_dop(H(:,1:3));% 输出结果fprintf('\n=== 定位结果 ===\n');fprintf('ECEF坐标: [%.3f, %.3f, %.3f] m\n', position);[lat, lon, alt] = ecef2lla(position');fprintf('经纬高: [%.6f°, %.6f°, %.3f m]\n', lat, lon, alt);fprintf('接收机钟差: %.3f m (%.3f ns)\n', receiver_clock_error, receiver_clock_error/0.299792458);fprintf('系统时间偏差: %.3f m\n', system_time_offset);fprintf('参与解算卫星: %d颗 (GPS: %d, BDS: %d)\n', ...sum(system_flags==1) + sum(system_flags==2), ...sum(system_flags==1), sum(system_flags==2));
end

2. 观测方程构建

function [H, delta_rho, sat_positions, sat_ids, system_flags] = ...build_observation_equation(rec_pos, rec_clock_error, sys_time_offset, ...obs_data, ephem_gps, ephem_bds)% 构建观测方程% 初始化变量num_gps = length(obs_data.gps.prn);num_bds = length(obs_data.bds.prn);total_sats = num_gps + num_bds;H = zeros(total_sats, 5); % [dx, dy, dz, c*dt_r, c*dt_sys]delta_rho = zeros(total_sats, 1);sat_positions = zeros(total_sats, 3);sat_ids = zeros(total_sats, 1);system_flags = zeros(total_sats, 1); % 1:GPS, 2:BDSsat_count = 0;% 处理GPS卫星for i = 1:num_gpsprn = obs_data.gps.prn(i);pseudorange = obs_data.gps.pseudo_range(i);% 计算卫星位置和钟差[sat_pos, sat_clock_error] = calculate_satellite_position(...prn, obs_data.gps.tow(i), ephem_gps, 'GPS');if ~isempty(sat_pos)sat_count = sat_count + 1;% 计算几何距离geometric_range = norm(sat_pos - rec_pos');% 计算视线向量line_of_sight = (sat_pos - rec_pos') / geometric_range;% 构建设计矩阵H(sat_count, 1:3) = -line_of_sight;H(sat_count, 4) = 1; % 接收机钟差H(sat_count, 5) = 0; % GPS系统该项为0% 计算伪距残差predicted_range = geometric_range + rec_clock_error - sat_clock_error;delta_rho(sat_count) = pseudorange - predicted_range;% 存储卫星信息sat_positions(sat_count, :) = sat_pos;sat_ids(sat_count) = prn;system_flags(sat_count) = 1;endend% 处理北斗卫星for i = 1:num_bdsprn = obs_data.bds.prn(i);pseudorange = obs_data.bds.pseudo_range(i);% 计算卫星位置和钟差[sat_pos, sat_clock_error] = calculate_satellite_position(...prn, obs_data.bds.tow(i), ephem_bds, 'BDS');if ~isempty(sat_pos)sat_count = sat_count + 1;% 计算几何距离geometric_range = norm(sat_pos - rec_pos');% 计算视线向量line_of_sight = (sat_pos - rec_pos') / geometric_range;% 构建设计矩阵H(sat_count, 1:3) = -line_of_sight;H(sat_count, 4) = 1; % 接收机钟差H(sat_count, 5) = 1; % 系统时间偏差% 计算伪距残差predicted_range = geometric_range + rec_clock_error + sys_time_offset - sat_clock_error;delta_rho(sat_count) = pseudorange - predicted_range;% 存储卫星信息sat_positions(sat_count, :) = sat_pos;sat_ids(sat_count) = prn;system_flags(sat_count) = 2;endend% 去除未使用的行H = H(1:sat_count, :);delta_rho = delta_rho(1:sat_count);sat_positions = sat_positions(1:sat_count, :);sat_ids = sat_ids(1:sat_count);system_flags = system_flags(1:sat_count);
end

3. 卫星位置计算

function [sat_position, sat_clock_error] = calculate_satellite_position(prn, tow, ephemeris, system)% 计算卫星位置和钟差% 基于广播星历计算% 查找对应PRN的星历eph_index = find([ephemeris.PRN] == prn);if isempty(eph_index)sat_position = [];sat_clock_error = 0;return;endeph = ephemeris(eph_index(1));% 系统常数if strcmp(system, 'GPS')mu = 3.986005e14;        % GPS地球引力常数 (m^3/s^2)omega_e = 7.2921151467e-5; % 地球自转角速度 (rad/s)else % BDSmu = 3.986004418e14;     % BDS地球引力常数omega_e = 7.2921150e-5;   % 地球自转角速度end% 计算信号发射时间t = tow - eph.Toc;% 卫星钟差计算sat_clock_error = eph.Af0 + eph.Af1 * t + eph.Af2 * t^2;% 轨道参数A = eph.sqrtA^2;          % 长半轴n0 = sqrt(mu / A^3);      % 平均角速度tk = t - eph.Toe;         % 相对于星历参考时间的时间差% 改正的平均角速度n = n0 + eph.DeltaN;% 平近点角M = eph.M0 + n * tk;% 偏近点角（开普勒方程迭代求解）E = M;for i = 1:10E_old = E;E = M + eph.e * sin(E);if abs(E - E_old) < 1e-12break;endend% 真近点角v = atan2(sqrt(1 - eph.e^2) * sin(E), cos(E) - eph.e);% 升交距角phi = v + eph.omega;% 摄动改正delta_u = eph.Cuc * cos(2*phi) + eph.Cus * sin(2*phi);delta_r = eph.Crc * cos(2*phi) + eph.Crs * sin(2*phi);delta_i = eph.Cic * cos(2*phi) + eph.Cis * sin(2*phi);% 改正后的升交距角、矢径、轨道倾角u = phi + delta_u;r = A * (1 - eph.e * cos(E)) + delta_r;i = eph.i0 + delta_i + eph.IDOT * tk;% 在轨道平面内的位置x_orb = r * cos(u);y_orb = r * sin(u);% 升交点经度Omega = eph.Omega0 + (eph.OmegaDot - omega_e) * tk - omega_e * eph.Toe;% 地固坐标系中的位置x_ecef = x_orb * cos(Omega) - y_orb * cos(i) * sin(Omega);y_ecef = x_orb * sin(Omega) + y_orb * cos(i) * cos(Omega);z_ecef = y_orb * sin(i);sat_position = [x_ecef, y_ecef, z_ecef];% 相对论效应改正sat_clock_error = sat_clock_error - 2 * sqrt(mu * A) * eph.e * sin(E) / 299792458^2;
end

4. 误差模型与权重计算

function W = calculate_weight_matrix(sat_positions, rec_pos, system_flags)% 计算权重矩阵（基于高度角）num_sats = size(sat_positions, 1);W = zeros(num_sats, num_sats);for i = 1:num_sats% 计算卫星高度角elevation = calculate_elevation(sat_positions(i,:), rec_pos);% 基于高度角的权重函数if elevation > 30weight = 1.0;elseif elevation > 15weight = 2 * sind(elevation);elseif elevation > 5weight = 0.5 * sind(elevation);elseweight = 0.1; % 低高度角卫星权重降低end% 系统间权重调整（可根据实际情况调整）if system_flags(i) == 2 % BDS卫星weight = weight * 1.0; % BDS权重因子endW(i,i) = weight;end
endfunction elevation = calculate_elevation(sat_pos, rec_pos)% 计算卫星高度角% 将接收机位置转换为经纬高[lat, lon, ~] = ecef2lla(rec_pos');% 计算ENU坐标系中的单位向量delta_pos = sat_pos - rec_pos';enu = ecef2enu(delta_pos, lat, lon, 0);% 计算高度角range = norm(enu);elevation = asind(enu(3) / range);
endfunction enu = ecef2enu(delta_pos, lat, lon, alt)% ECEF到ENU坐标转换phi = deg2rad(lat);lambda = deg2rad(lon);R = [-sin(lambda), cos(lambda), 0; ...-sin(phi)*cos(lambda), -sin(phi)*sin(lambda), cos(phi); ...cos(phi)*cos(lambda), cos(phi)*sin(lambda), sin(phi)];enu = R * delta_pos';
end

5. 精度评估与DOP计算

function dop = calculate_dop(H_geometry)% 计算精度因子% 几何矩阵G = H_geometry;% 协因数矩阵Q = inv(G' * G);% 各种精度因子dop.GDOP = sqrt(trace(Q));                    % 几何精度因子dop.PDOP = sqrt(Q(1,1) + Q(2,2) + Q(3,3));   % 位置精度因子dop.HDOP = sqrt(Q(1,1) + Q(2,2));            % 水平精度因子dop.VDOP = sqrt(Q(3,3));                     % 垂直精度因子dop.TDOP = sqrt(Q(4,4));                     % 时间精度因子fprintf('\n=== 精度因子 ===\n');fprintf('GDOP: %.3f\n', dop.GDOP);fprintf('PDOP: %.3f\n', dop.PDOP);fprintf('HDOP: %.3f\n', dop.HDOP);fprintf('VDOP: %.3f\n', dop.VDOP);fprintf('TDOP: %.3f\n', dop.TDOP);
endfunction cov_matrix = calculate_covariance_matrix(H, W, residuals)% 计算协方差矩阵num_obs = size(H, 1);num_params = size(H, 2);% 验后单位权方差v = residuals;sigma2 = (v' * W * v) / (num_obs - num_params);% 协方差矩阵cov_matrix = sigma2 * inv(H' * W * H);
end

6. 数据模拟与性能分析

function [obs_data, ephem_gps, ephem_bds, true_position] = simulate_gnss_data()% 模拟GPS/北斗观测数据% 真实位置（北京）true_position = [-2148744, 4426641, 4044655]; % ECEF坐标 (m)% 模拟时间tow = 0:30:86400; % 一整天的观测，每30秒一个历元% 初始化观测数据结构obs_data.gps.tow = [];obs_data.gps.prn = [];obs_data.gps.pseudo_range = [];obs_data.bds.tow = [];obs_data.bds.prn = [];obs_data.bds.pseudo_range = [];% 生成GPS星历（简化版）ephem_gps = generate_ephemeris('GPS', 32);% 生成北斗星历ephem_bds = generate_ephemeris('BDS', 35);fprintf('生成模拟GNSS数据...\n');for t = 1:length(tow)current_tow = tow(t);% GPS卫星观测visible_gps = find_visible_satellites(true_position, ephem_gps, current_tow, 'GPS');for i = 1:length(visible_gps)prn = visible_gps(i);[sat_pos, sat_clock] = calculate_satellite_position(prn, current_tow, ephem_gps, 'GPS');% 计算真实几何距离true_range = norm(sat_pos - true_position);% 模拟伪距（添加误差）pseudo_range = true_range + ...2.0 * randn + ...        % 接收机噪声0.1 * true_range/20000 + ... % 与距离相关的误差sat_clock;               % 卫星钟差% 存储观测数据obs_data.gps.tow(end+1) = current_tow;obs_data.gps.prn(end+1) = prn;obs_data.gps.pseudo_range(end+1) = pseudo_range;end% 北斗卫星观测visible_bds = find_visible_satellites(true_position, ephem_bds, current_tow, 'BDS');for i = 1:length(visible_bds)prn = visible_bds(i);[sat_pos, sat_clock] = calculate_satellite_position(prn, current_tow, ephem_bds, 'BDS');% 计算真实几何距离true_range = norm(sat_pos - true_position);% 模拟伪距（添加误差）pseudo_range = true_range + ...2.5 * randn + ...        % 接收机噪声（BDS稍大）0.1 * true_range/20000 + ... % 与距离相关的误差sat_clock;               % 卫星钟差% 存储观测数据obs_data.bds.tow(end+1) = current_tow;obs_data.bds.prn(end+1) = prn;obs_data.bds.pseudo_range(end+1) = pseudo_range;endendfprintf('数据生成完成: GPS观测%d个, BDS观测%d个\n', ...length(obs_data.gps.tow), length(obs_data.bds.tow));
endfunction visible_sats = find_visible_satellites(rec_pos, ephemeris, tow, system)% 查找可见卫星visible_sats = [];elevation_mask = 5; % 高度角掩模（度）for i = 1:length(ephemeris)prn = ephemeris(i).PRN;[sat_pos, ~] = calculate_satellite_position(prn, tow, ephemeris, system);if ~isempty(sat_pos)elevation = calculate_elevation(sat_pos, rec_pos');if elevation > elevation_maskvisible_sats(end+1) = prn;endendend
end

7. 性能比较与分析

function compare_positioning_performance()% 比较单系统与组合系统定位性能% 生成模拟数据[obs_data, ephem_gps, ephem_bds, true_position] = simulate_gnss_data();% 初始位置估计init_pos = true_position' + [1000; 1000; 1000]; % 添加1km初始误差fprintf('\n=== GPS单系统定位 ===\n');[pos_gps, cov_gps, res_gps, dop_gps] = gps_only_spp(obs_data, ephem_gps, init_pos);fprintf('\n=== BDS单系统定位 ===\n');[pos_bds, cov_bds, res_bds, dop_bds] = bds_only_spp(obs_data, ephem_bds, init_pos);fprintf('\n=== GPS/BDS组合定位 ===\n');[pos_comb, cov_comb, res_comb, dop_comb] = gps_bds_spp(obs_data, ephem_gps, ephem_bds, init_pos);% 计算误差error_gps = norm(pos_gps - true_position');error_bds = norm(pos_bds - true_position');error_comb = norm(pos_comb - true_position');% 可视化结果figure('Position', [100, 100, 1200, 800]);% 定位误差比较subplot(2,3,1);errors = [error_gps, error_bds, error_comb];bar(errors);set(gca, 'XTickLabel', {'GPS', 'BDS', '组合'});ylabel('定位误差 (m)');title('定位误差比较');grid on;% DOP值比较subplot(2,3,2);dop_values = [dop_gps.PDOP, dop_bds.PDOP, dop_comb.PDOP;dop_gps.HDOP, dop_bds.HDOP, dop_comb.HDOP;dop_gps.VDOP, dop_bds.VDOP, dop_comb.VDOP];bar(dop_values');set(gca, 'XTickLabel', {'GPS', 'BDS', '组合'});ylabel('DOP值');title('精度因子比较');legend('PDOP', 'HDOP', 'VDOP', 'Location', 'best');grid on;% 卫星数量统计subplot(2,3,3);gps_sats = length(unique(obs_data.gps.prn));bds_sats = length(unique(obs_data.bds.prn));total_sats = gps_sats + bds_sats;satellite_counts = [gps_sats, bds_sats, total_sats];pie(satellite_counts, {'GPS', 'BDS', '总计'});title('可见卫星数量分布');% 残差分析subplot(2,3,4);histogram(res_gps, 50, 'FaceAlpha', 0.7);hold on;histogram(res_bds, 50, 'FaceAlpha', 0.7);histogram(res_comb, 50, 'FaceAlpha', 0.7);xlabel('残差 (m)');ylabel('频数');title('伪距残差分布');legend('GPS', 'BDS', '组合', 'Location', 'best');grid on;% 误差椭圆subplot(2,3,5);plot_error_ellipse(pos_gps(1:2), cov_gps(1:2,1:2), 'r');hold on;plot_error_ellipse(pos_bds(1:2), cov_bds(1:2,1:2), 'g');plot_error_ellipse(pos_comb(1:2), cov_comb(1:2,1:2), 'b');plot(true_position(1), true_position(2), 'kx', 'MarkerSize', 10, 'LineWidth', 2);xlabel('X (m)');ylabel('Y (m)');title('水平误差椭圆 (95%置信)');legend('GPS', 'BDS', '组合', '真实位置', 'Location', 'best');grid on;axis equal;% 收敛曲线subplot(2,3,6);% 这里可以添加收敛过程的记录和绘图fprintf('\n=== 性能总结 ===\n');fprintf('GPS单系统定位误差: %.3f m\n', error_gps);fprintf('BDS单系统定位误差: %.3f m\n', error_bds);fprintf('GPS/BDS组合定位误差: %.3f m\n', error_comb);fprintf('精度提升: %.1f%%\n', (error_gps - error_comb)/error_gps*100);
endfunction plot_error_ellipse(center, cov_matrix, color)% 绘制误差椭圆% 置信水平对应的卡方值（2自由度，95%置信）k = sqrt(5.991);% 特征值分解[V, D] = eig(cov_matrix);% 椭圆参数a = k * sqrt(D(1,1)); % 长半轴b = k * sqrt(D(2,2)); % 短半轴angle = atan2(V(2,1), V(1,1)); % 旋转角% 生成椭圆点theta = linspace(0, 2*pi, 100);ellipse = [a * cos(theta); b * sin(theta)];% 旋转R = [cos(angle), -sin(angle); sin(angle), cos(angle)];ellipse = R * ellipse;% 平移ellipse(1,:) = ellipse(1,:) + center(1);ellipse(2,:) = ellipse(2,:) + center(2);% 绘制plot(ellipse(1,:), ellipse(2,:), color, 'LineWidth', 2);plot(center(1), center(2), [color 'o'], 'MarkerSize', 4);
end

使用

基本使用方法

% 运行完整性能比较
compare_positioning_performance();% 或者单独使用组合定位
[obs_data, ephem_gps, ephem_bds, true_pos] = simulate_gnss_data();
init_pos = [0; 0; 0]; % 粗略初始位置[pos, cov, res, dop] = gps_bds_spp(obs_data, ephem_gps, ephem_bds, init_pos);% 转换为经纬高
[lat, lon, alt] = ecef2lla(pos');
fprintf('定位结果: 纬度=%.6f°, 经度=%.6f°, 高程=%.2fm\n', lat, lon, alt);