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

基于松组合和紧组合的GPS/SINS组合导航MATLAB仿真验证代码

news 2026/3/26 20:55:55

一、系统架构设计

1. 组合模式对比

参数	松组合 (Loose Coupling)	紧组合 (Tight Coupling)
量测类型	位置/速度差 (ΔP, ΔV)	伪距/伪距率差 (Δρ, ΔṖ)
更新频率	1Hz (GPS与INS独立更新)	10Hz (同步更新)
精度	13-16m (水平)	3-5m (水平)
实现复杂度	低 (独立解算)	高 (需伪距计算)

二、仿真代码

1. 参数初始化

%% 系统参数定义
global dt;
dt = 0.1; % 时间步长 (s)% 导航坐标系参数
Re = 6378245; % 地球长半径 (m)
g0 = 9.78049; % 重力加速度 (m/s²)
omega_ie = 7.2921158e-5; % 地球自转角速度 (rad/s)% 噪声配置
Q_ins = diag([0.01^2, 0.01^2, 0.01^2, 0.001^2, 0.001^2, 0.001^2]); % INS过程噪声
Q_gps = diag([10^2, 10^2, 10^2]); % GPS过程噪声
R_ins = diag([1e-4, 1e-4, 1e-4]); % INS测量噪声
R_gps = diag([3^2, 3^2, 3^2]); % GPS测量噪声

2. 状态方程建模

%% 状态向量定义 (松组合)
% [x, y, z, vx, vy, vz, phi, theta, psi, bx, by, bz, wx, wy, wz]
x = zeros(15,1);%% 状态转移矩阵 (连续时间)
F = [...0 0 0 1 0 0 0 0 0 0 0 0 0 0 0;0 0 0 0 1 0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 1 0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 -g0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 -g0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 0 -g0 0 0 0;1 0 0 0 0 0 0 0 0 0 0 0 0 0 0;0 1 0 0 0 0 0 0 0 0 0 0 0 0 0;0 0 1 0 0 0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 0 0 0 0 0];%% 离散化处理
Phi = expm(F*dt);

3. 松组合卡尔曼滤波

%% 松组合滤波流程
function [x_est, P] = loose_coupling_filter(x, P, z_gps, R_gps)global dt;% 预测步骤x_pred = Phi * x;P_pred = Phi * P * Phi' + Q_ins * dt;% 量测更新H = [eye(3), zeros(3,12)]; % 位置量测矩阵K = P_pred * H' / (H * P_pred * H' + R_gps);x_est = x_pred + K * (z_gps - x_pred(1:3));P = (eye(15) - K*H) * P_pred;
end

4. 紧组合伪距计算

%% 伪距计算函数
function rho = compute_pseudorange(x_ins, sv_pos)% x_ins: INS位置 (ECEF坐标系)% sv_pos: 卫星位置 (ECEF坐标系)rho = sqrt(sum((x_ins - sv_pos).^2)) + ...2.5e-9 * ones(size(x_ins,1),1) + ... % 时钟偏差1.5e-12 * (norm(x_ins - sv_pos)).^2; % 相对论修正
end

5. 紧组合卡尔曼滤波

%% 紧组合滤波流程
function [x_est, P] = tight_coupling_filter(x, P, z_rho, R_rho)global dt;% 预测步骤x_pred = Phi * x;P_pred = Phi * P * Phi' + Q_ins * dt;% 量测更新H = [zeros(3,3), eye(3), zeros(3,9), ... % 伪距量测矩阵zeros(3,3), zeros(3,3), eye(3), ... zeros(3,3), zeros(3,3), zeros(3,3)];K = P_pred * H' / (H * P_pred * H' + R_rho);x_est = x_pred + K * (z_rho - compute_pseudorange(x_pred(4:6), sv_pos));P = (eye(15) - K*H) * P_pred;
end

三、仿真验证流程

1. 数据生成

%% 生成真实轨迹
t = 0:dt:100;
[x_true, v_true, a_true] = generate_trajectory(t); % 包含机动段%% 生成带噪声的INS数据
[acc, gyro] = imu_noise_model(a_true, omega_true); % 添加传感器噪声
[x_ins, v_ins] = ins_integrate(acc, gyro, dt); % 积分得到INS解算结果%% 生成GPS测量值
sv_pos = get_satellite_positions(t); % 获取卫星ECEF坐标
z_gps = x_true(1:3) + 3*randn(3,length(t)); % 位置测量
z_rho = compute_pseudorange(x_ins(4:6,:), sv_pos); % 伪距测量

2. 滤波仿真

%% 松组合仿真
x_est_loose = zeros(15,length(t));
P_loose = eye(15);
for k = 1:length(t)[x_est_loose(:,k), P_loose] = loose_coupling_filter(...x_est_loose(:,k-1), P_loose, z_gps(:,k), R_gps);
end%% 紧组合仿真
x_est_tight = zeros(15,length(t));
P_tight = eye(15);
for k = 1:length(t)[x_est_tight(:,k), P_tight] = tight_coupling_filter(...x_est_tight(:,k-1), P_tight, z_rho(:,k), R_rho);
end

四、结果分析

1. 位置误差对比

figure;
subplot(3,1,1);
plot(t, x_true(1,:) - x_est_loose(1,:), 'r', ...t, x_true(1,:) - x_est_tight(1,:), 'g');
title('东向位置误差');
xlabel('时间(s)'); ylabel('误差(m)');subplot(3,1,2);
plot(t, x_true(2,:) - x_est_loose(2,:), 'r', ...t, x_true(2,:) - x_est_tight(2,:), 'g');
title('北向位置误差');subplot(3,1,3);
plot(t, x_true(3,:) - x_est_loose(3,:), 'r', ...t, x_true(3,:) - x_est_tight(3,:), 'g');
title('天向位置误差');