惯性导航精解算程序（MATLAB实现）

一、系统架构与数学模型

1.1 惯性导航系统构成

┌─────────────────────────────────────────────────────────────┐
│                    惯性导航精解算系统                        │
├─────────────────────────────────────────────────────────────┤
│  传感器数据  │  初始对准  │  力学编排  │  误差补偿  │  输出  │
│  (IMU)      │  (初始姿态) │  (导航解算) │  (卡尔曼)  │  (位置/姿态) │
├─────────────┼────────────┼────────────┼────────────┼──────────┤
│  • 陀螺仪   │  • 静止对准 │  • 姿态更新 │  • 零偏估计 │  • 经度   │
│  • 加速度计 │  • 动对准   │  • 速度更新 │  • 标度因数 │  • 纬度   │
│  • 温度计   │  • 传递对准 │  • 位置更新 │  • 安装误差 │  • 高度   │
└─────────────────────────────────────────────────────────────┘

1.2 坐标系定义

坐标系	符号	描述
地心地固坐标系	ECEF	原点在地心，Z轴指向北极，X轴指向格林威治子午线与赤道交点
当地水平坐标系	LLH	经度(Longitude)、纬度(Latitude)、高度(Height)
导航坐标系	n系	东-北-天(ENU)或北-东-地(NED)
载体坐标系	b系	右-前-上(X-Y-Z)
惯性坐标系	i系	不随地球旋转的惯性空间坐标系

---

二、MATLAB实现代码

2.1 主程序：ins_precise_navigation.m

%% 惯性导航精解算程序
% 功能：基于IMU数据的精密导航解算，包含初始对准、力学编排和误差补偿

clear; clc; close all;

%% 1. 系统参数配置
fprintf('=== 惯性导航精解算系统 ===\n');
params = configure_ins_parameters();

fprintf('系统配置:\n');
fprintf('  导航坐标系: %s\n', params.nav_frame);
fprintf('  地球模型: %s\n', params.earth_model);
fprintf('  解算频率: %.1f Hz\n', params.fs);
fprintf('  总时长: %.1f 秒\n', params.duration);
fprintf('  初始位置: 纬度=%.6f°, 经度=%.6f°, 高度=%.1f m\n', ...
        params.init_lat*180/pi, params.init_lon*180/pi, params.init_alt);

%% 2. 生成/加载IMU数据
fprintf('\n生成IMU仿真数据...\n');
imu_data = generate_imu_data(params);

%% 3. 初始对准
fprintf('执行初始对准...\n');
tic;
initial_attitude = initial_alignment(imu_data, params);
align_time = toc;
fprintf('  初始对准完成！耗时: %.3f秒\n', align_time);
fprintf('  初始姿态角: 俯仰=%.4f°, 横滚=%.4f°, 航向=%.4f°\n', ...
        initial_attitude.pitch*180/pi, initial_attitude.roll*180/pi, initial_attitude.yaw*180/pi);

%% 4. 惯性导航精解算
fprintf('执行惯性导航精解算...\n');
tic;
navigation_result = ins_mechanization(imu_data, initial_attitude, params);
mech_time = toc;
fprintf('  力学编排完成！耗时: %.3f秒\n', mech_time);

%% 5. 误差补偿（卡尔曼滤波）
fprintf('执行误差补偿...\n');
tic;
[kf_states, kf_covariance] = kalman_filter_compensation(navigation_result, imu_data, params);
kf_time = toc;
fprintf('  卡尔曼滤波完成！耗时: %.3f秒\n', kf_time);

%% 6. 轨迹生成与真值对比
fprintf('生成参考轨迹...\n');
reference_trajectory = generate_reference_trajectory(params);

%% 7. 性能评估
fprintf('性能评估...\n');
evaluate_ins_performance(navigation_result, reference_trajectory, params);

%% 8. 可视化结果
visualize_ins_results(navigation_result, reference_trajectory, kf_states, params);

2.2 参数配置：configure_ins_parameters.m

function params = configure_ins_parameters()
    % 配置惯性导航系统参数
    
    %% 基本参数
    params.fs = 200;                 % IMU采样频率 (Hz)
    params.duration = 300;            % 导航时长 (秒)
    params.nav_frame = 'ENU';         % 导航坐标系: 'ENU'(东-北-天) 或 'NED'(北-东-地)
    params.earth_model = 'WGS84';     % 地球模型
    
    %% 初始位置 (WGS84坐标系)
    params.init_lat = 34.0522 * pi/180;  % 初始纬度 (弧度) - 洛杉矶
    params.init_lon = -118.2437 * pi/180; % 初始经度 (弧度)
    params.init_alt = 100;              % 初始高度 (米)
    
    %% 初始速度 (m/s)
    params.init_ve = 0;               % 东向速度
    params.init_vn = 10;              % 北向速度
    params.init_vu = 0;               % 天向速度
    
    %% 初始姿态 (弧度)
    params.init_pitch = 0;            % 俯仰角
    params.init_roll = 0;             % 横滚角
    params.init_yaw = 45 * pi/180;    % 航向角 (45度)
    
    %% 地球参数 (WGS84)
    params.Re = 6378137;              % 地球赤道半径 (m)
    params.Rp = 6356752.3142;         % 地球极半径 (m)
    params.e = sqrt(1 - (params.Rp/params.Re)^2); % 第一偏心率
    params.omega_ie = 7.2921151467e-5; % 地球自转角速度 (rad/s)
    params.mu = 3.986004418e14;       % 地球引力常数 (m^3/s^2)
    params.g0 = 9.780327;             % 标准重力加速度 (m/s^2)
    
    %% IMU误差参数
    params.gyro_bias = 0.01 * pi/180/3600;    % 陀螺零偏 (rad/s) - 0.01 deg/h
    params.gyro_arw = 0.001 * pi/180/sqrt(3600); % 角随机游走 (rad/√s)
    params.accel_bias = 100e-6 * 9.8;     % 加速度计零偏 (m/s^2) - 100 μg
    params.accel_vrw = 0.01;             % 速度随机游走 (m/s/√s)
    
    %% 安装误差
    params.misalign_angles = [0.1, 0.1, 0.1] * pi/180; % 安装误差角 (弧度)
    
    %% 卡尔曼滤波参数
    params.kf_q = diag([0.01, 0.01, 0.01, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001]); % 过程噪声
    params.kf_r = diag([0.1, 0.1, 0.1, 0.01, 0.01, 0.01]); % 观测噪声
    
    fprintf('INS参数配置完成！\n');
end

2.3 IMU数据生成：generate_imu_data.m

function imu_data = generate_imu_data(params)
    % 生成仿真IMU数据
    
    N = params.fs * params.duration;  % 总采样点数
    dt = 1/params.fs;                 % 采样间隔
    
    % 初始化IMU数据结构
    imu_data = struct();
    imu_data.time = (0:N-1)' * dt;
    imu_data.gyro = zeros(N, 3);      % 陀螺仪数据 [ωx, ωy, ωz] (rad/s)
    imu_data.accel = zeros(N, 3);     % 加速度计数据 [ax, ay, az] (m/s^2)
    imu_data.temp = 25 + 5*randn(N, 1); % 温度数据 (°C)
    
    % 生成载体运动轨迹
    [trajectory, attitude] = generate_vehicle_trajectory(N, dt, params);
    
    % 计算理想IMU输出
    for k = 1:N
        % 姿态四元数
        q = attitude.quaternion(k, :)';
        
        % 载体角速度 (导航系到载体系的旋转)
        omega_nb = trajectory.angular_velocity(k, :)';
        
        % 转换到载体系
        omega_b = quatrotate(q, omega_nb);
        
        % 添加地球自转补偿
        omega_ie = [0; params.omega_ie; 0];  % 导航系地球自转角速度
        omega_ie_b = quatrotate(q, omega_ie);
        
        % 陀螺仪测量值
        imu_data.gyro(k, :) = (omega_b + omega_ie_b)' + params.gyro_bias;
        
        % 载体比力 (导航系)
        specific_force_n = trajectory.specific_force(k, :)';
        
        % 转换到载体系
        specific_force_b = quatrotate(q, specific_force_n);
        
        % 加速度计测量值
        imu_data.accel(k, :) = specific_force_b' + params.accel_bias;
        
        % 添加噪声
        imu_data.gyro(k, :) = imu_data.gyro(k, :) + params.gyro_arw * randn(1,3) / sqrt(dt);
        imu_data.accel(k, :) = imu_data.accel(k, :) + params.accel_vrw * randn(1,3) / sqrt(dt);
    end
    
    fprintf('  IMU数据生成完成: %d 个采样点\n', N);
end

function [trajectory, attitude] = generate_vehicle_trajectory(N, dt, params)
    % 生成车辆运动轨迹
    
    % 初始化
    trajectory = struct();
    attitude = struct();
    
    % 位置 (LLH)
    lat = params.init_lat * ones(N, 1);
    lon = params.init_lon * ones(N, 1);
    alt = params.init_alt * ones(N, 1);
    
    % 速度 (ENU)
    ve = params.init_ve * ones(N, 1);
    vn = params.init_vn * ones(N, 1);
    vu = params.init_vu * ones(N, 1);
    
    % 姿态
    pitch = params.init_pitch * ones(N, 1);
    roll = params.init_roll * ones(N, 1);
    yaw = params.init_yaw * ones(N, 1);
    
    % 角速度
    omega_x = zeros(N, 1);
    omega_y = zeros(N, 1);
    omega_z = zeros(N, 1);
    
    % 比力 (导航系)
    fn_e = zeros(N, 1);
    fn_n = zeros(N, 1);
    fn_u = 9.8 * ones(N, 1);  % 重力
    
    % 生成机动轨迹
    for k = 2:N
        % 时间
        t = (k-1) * dt;
        
        % 速度变化 (正弦机动)
        ve(k) = params.init_ve + 5 * sin(0.1*t);
        vn(k) = params.init_vn + 3 * sin(0.05*t);
        vu(k) = params.init_vu + 0.5 * sin(0.2*t);
        
        % 姿态变化
        pitch(k) = params.init_pitch + 0.05 * sin(0.1*t);
        roll(k) = params.init_roll + 0.03 * sin(0.08*t);
        yaw(k) = params.init_yaw + 0.1 * sin(0.05*t);
        
        % 角速度 (欧拉角导数)
        if k > 1
            dt_small = dt;
            omega_x(k) = (pitch(k) - pitch(k-1)) / dt_small;
            omega_y(k) = (roll(k) - roll(k-1)) / dt_small;
            omega_z(k) = (yaw(k) - yaw(k-1)) / dt_small;
        end
        
        % 比力 (考虑向心加速度和哥氏加速度)
        fn_e(k) = (vn(k) - vn(k-1)) / dt_small - params.omega_ie * sin(lat(k)) * ve(k);
        fn_n(k) = (ve(k) - ve(k-1)) / dt_small + params.omega_ie * sin(lat(k)) * vn(k);
        fn_u(k) = (vu(k) - vu(k-1)) / dt_small - params.g0;
    end
    
    % 更新位置 (经纬度)
    for k = 2:N
        dt_small = dt;
        % 地球半径随纬度变化
        Re_h = params.Re / sqrt(1 - params.e^2 * sin(lat(k))^2);
        Rn_h = params.Re * (1 - params.e^2) / (1 - params.e^2 * sin(lat(k))^2)^(3/2);
        
        % 位置更新
        lat(k) = lat(k-1) + vn(k) * dt_small / (Rn_h + alt(k));
        lon(k) = lon(k-1) + ve(k) * dt_small / ((Re_h + alt(k)) * cos(lat(k)));
        alt(k) = alt(k-1) + vu(k) * dt_small;
    end
    
    % 四元数
    quaternion = zeros(N, 4);
    for k = 1:N
        quaternion(k, :) = euler2quat([pitch(k), roll(k), yaw(k)]);
    end
    
    % 整理输出
    trajectory.lat = lat;
    trajectory.lon = lon;
    trajectory.alt = alt;
    trajectory.ve = ve;
    trajectory.vn = vn;
    trajectory.vu = vu;
    trajectory.specific_force = [fn_e, fn_n, fn_u];
    trajectory.angular_velocity = [omega_x, omega_y, omega_z];
    
    attitude.pitch = pitch;
    attitude.roll = roll;
    attitude.yaw = yaw;
    attitude.quaternion = quaternion;
end

function q = euler2quat(euler)
    % 欧拉角转四元数
    pitch = euler(1);
    roll = euler(2);
    yaw = euler(3);
    
    cy = cos(yaw/2);
    sy = sin(yaw/2);
    cr = cos(roll/2);
    sr = sin(roll/2);
    cp = cos(pitch/2);
    sp = sin(pitch/2);
    
    q = [cy*cr*cp + sy*sr*sp, ...
         cy*cr*sp - sy*sr*cp, ...
         cy*sr*cp + sy*cr*sp, ...
         sy*cr*cp - cy*sr*sp];
end

2.4 初始对准：initial_alignment.m

function initial_attitude = initial_alignment(imu_data, params)
    % 初始对准（静基座对准）
    
    fprintf('  执行静基座初始对准...\n');
    
    % 提取数据
    gyro = imu_data.gyro;
    accel = imu_data.accel;
    N = size(gyro, 1);
    
    % 1. 粗对准：利用重力矢量确定俯仰和横滚
    fprintf('    粗对准...\n');
    
    % 平均加速度
    accel_mean = mean(accel(1:min(1000,N), :), 1);
    
    % 计算俯仰角和横滚角
    ax = accel_mean(1);
    ay = accel_mean(2);
    az = accel_mean(3);
    
    pitch = atan2(-ax, sqrt(ay^2 + az^2));
    roll = atan2(ay, az);
    
    % 2. 精对准：利用陀螺仪数据确定航向角
    fprintf('    精对准...\n');
    
    % 计算地球自转角速度在载体系的投影
    % 假设初始位置已知
    lat = params.init_lat;
    omega_ie = params.omega_ie;
    
    % 地球自转角速度在导航系的投影
    omega_ie_n = [0; omega_ie * cos(lat); omega_ie * sin(lat)];
    
    % 转换到载体系（使用粗对准的姿态）
    coarse_quat = euler2quat([pitch, roll, 0]);
    omega_ie_b = quatrotate(coarse_quat, omega_ie_n);
    
    % 从陀螺仪数据中估计航向角
    % 陀螺仪测量值 = 地球自转 + 载体运动
    % 静基座下，载体运动为0，所以陀螺仪测量值 = 地球自转在载体系的投影
    
    % 使用最小二乘法估计航向角
    yaw_estimates = zeros(N, 1);
    for k = 1:N
        % 陀螺仪测量值
        gyro_meas = gyro(k, :)';
        
        % 地球自转在载体系的投影（假设航向角为0）
        omega_ie_b_est = quatrotate(coarse_quat, omega_ie_n);
        
        % 计算航向角误差
        yaw_error = atan2(gyro_meas(2) - omega_ie_b_est(2), gyro_meas(1) - omega_ie_b_est(1));
        yaw_estimates(k) = yaw_error;
    end
    
    % 取平均值作为航向角估计
    yaw = mean(yaw_estimates(100:end));
    
    % 3. 安装误差补偿
    fprintf('    安装误差补偿...\n');
    pitch = pitch + params.misalign_angles(1);
    roll = roll + params.misalign_angles(2);
    yaw = yaw + params.misalign_angles(3);
    
    % 输出初始姿态
    initial_attitude.pitch = pitch;
    initial_attitude.roll = roll;
    initial_attitude.yaw = yaw;
    initial_attitude.quaternion = euler2quat([pitch, roll, yaw]);
    
    fprintf('    对准完成: 俯仰=%.4f°, 横滚=%.4f°, 航向=%.4f°\n', ...
            pitch*180/pi, roll*180/pi, yaw*180/pi);
end

2.5 力学编排：ins_mechanization.m

function navigation_result = ins_mechanization(imu_data, initial_attitude, params)
    % 惯性导航力学编排（精解算）
    
    fprintf('  执行力学编排...\n');
    
    N = size(imu_data.gyro, 1);
    dt = 1/params.fs;
    
    % 初始化导航参数
    nav = struct();
    nav.time = zeros(N, 1);
    nav.lat = zeros(N, 1);
    nav.lon = zeros(N, 1);
    nav.alt = zeros(N, 1);
    nav.ve = zeros(N, 1);
    nav.vn = zeros(N, 1);
    nav.vu = zeros(N, 1);
    nav.pitch = zeros(N, 1);
    nav.roll = zeros(N, 1);
    nav.yaw = zeros(N, 1);
    nav.quaternion = zeros(N, 4);
    
    % 设置初始值
    nav.time(1) = 0;
    nav.lat(1) = params.init_lat;
    nav.lon(1) = params.init_lon;
    nav.alt(1) = params.init_alt;
    nav.ve(1) = params.init_ve;
    nav.vn(1) = params.init_vn;
    nav.vu(1) = params.init_vu;
    nav.pitch(1) = initial_attitude.pitch;
    nav.roll(1) = initial_attitude.roll;
    nav.yaw(1) = initial_attitude.yaw;
    nav.quaternion(1, :) = initial_attitude.quaternion;
    
    % 地球参数
    Re = params.Re;
    Rp = params.Rp;
    e = params.e;
    omega_ie = params.omega_ie;
    g0 = params.g0;
    
    % 力学编排主循环
    for k = 2:N
        % 1. 姿态更新（四元数法）
        % 陀螺仪数据
        gyro_b = imu_data.gyro(k-1, :)';  % 载体系角速度
        
        % 地球自转角速度在导航系的投影
        lat_k = nav.lat(k-1);
        omega_ie_n = [0; omega_ie * cos(lat_k); omega_ie * sin(lat_k)];
        
        % 地球自转角速度在载体系的投影
        q_k = nav.quaternion(k-1, :)';
        omega_ie_b = quatrotate(q_k, omega_ie_n);
        
        % 总角速度（载体系）
        omega_total_b = gyro_b - omega_ie_b;
        
        % 四元数微分方程
        Omega = [0, -omega_total_b(1), -omega_total_b(2), -omega_total_b(3);
                 omega_total_b(1), 0, omega_total_b(3), -omega_total_b(2);
                 omega_total_b(2), -omega_total_b(3), 0, omega_total_b(1);
                 omega_total_b(3), omega_total_b(2), -omega_total_b(1), 0];
        
        % 四元数更新
        q_dot = 0.5 * Omega * q_k;
        q_new = q_k + q_dot * dt;
        q_new = q_new / norm(q_new);  % 归一化
        
        nav.quaternion(k, :) = q_new';
        
        % 提取欧拉角
        [pitch, roll, yaw] = quat2euler(q_new);
        nav.pitch(k) = pitch;
        nav.roll(k) = roll;
        nav.yaw(k) = yaw;
        
        % 2. 速度更新
        % 加速度计数据
        accel_b = imu_data.accel(k-1, :)';  % 载体系比力
        
        % 转换到导航系
        accel_n = quatrotate(quatconj(q_new), accel_b);
        
        % 重力补偿
        gn = [0; 0; -g0];  % 导航系重力矢量
        
        % 哥氏加速度补偿
        ve = nav.ve(k-1);
        vn = nav.vn(k-1);
        vu = nav.vu(k-1);
        v_n = [ve; vn; vu];
        
        coriolis_accel = 2 * cross(omega_ie_n, v_n);
        
        % 向心加速度补偿
        centri_accel = cross(omega_ie_n, cross(omega_ie_n, [0; 0; nav.alt(k-1)]));
        
        % 导航系加速度
        a_n = accel_n + gn - coriolis_accel - centri_accel;
        
        % 速度更新
        nav.ve(k) = nav.ve(k-1) + a_n(1) * dt;
        nav.vn(k) = nav.vn(k-1) + a_n(2) * dt;
        nav.vu(k) = nav.vu(k-1) + a_n(3) * dt;
        
        % 3. 位置更新
        % 地球半径随纬度变化
        Re_h = Re / sqrt(1 - e^2 * sin(lat_k)^2);
        Rn_h = Re * (1 - e^2) / (1 - e^2 * sin(lat_k)^2)^(3/2);
        
        % 位置更新
        nav.lat(k) = nav.lat(k-1) + nav.vn(k) * dt / (Rn_h + nav.alt(k-1));
        nav.lon(k) = nav.lon(k-1) + nav.ve(k) * dt / ((Re_h + nav.alt(k-1)) * cos(nav.lat(k-1)));
        nav.alt(k) = nav.alt(k-1) + nav.vu(k) * dt;
        
        nav.time(k) = nav.time(k-1) + dt;
    end
    
    navigation_result = nav;
    fprintf('  力学编排完成: %d 个历元\n', N);
end

%% 四元数工具函数
function q_conj = quatconj(q)
    % 四元数共轭
    q_conj = [q(1), -q(2), -q(3), -q(4)];
end

function v_b = quatrotate(q, v_n)
    % 四元数旋转：导航系矢量 -> 载体系矢量
    % q: 导航系到载体系的旋转四元数
    % v_n: 导航系矢量
    
    % 构造纯四元数
    v_q = [0, v_n(1), v_n(2), v_n(3)];
    
    % 旋转
    v_q_rot = quatmultiply(quatmultiply(q, v_q), quatconj(q));
    
    v_b = v_q_rot(2:4);
end

function q = quatmultiply(q1, q2)
    % 四元数乘法
    w1 = q1(1); x1 = q1(2); y1 = q1(3); z1 = q1(4);
    w2 = q2(1); x2 = q2(2); y2 = q2(3); z2 = q2(4);
    
    w = w1*w2 - x1*x2 - y1*y2 - z1*z2;
    x = w1*x2 + x1*w2 + y1*z2 - z1*y2;
    y = w1*y2 - x1*z2 + y1*w2 + z1*x2;
    z = w1*z2 + x1*y2 - y1*x2 + z1*w2;
    
    q = [w, x, y, z];
end

function [pitch, roll, yaw] = quat2euler(q)
    % 四元数转欧拉角
    w = q(1); x = q(2); y = q(3); z = q(4);
    
    % 俯仰角 (pitch)
    pitch = atan2(2*(w*y - z*x), 1 - 2*(y^2 + x^2));
    
    % 横滚角 (roll)
    roll = asin(2*(w*x + y*z));
    
    % 航向角 (yaw)
    yaw = atan2(2*(w*z - x*y), 1 - 2*(x^2 + z^2));
end

2.6 卡尔曼滤波误差补偿：kalman_filter_compensation.m

function [kf_states, kf_covariance] = kalman_filter_compensation(navigation_result, imu_data, params)
    % 卡尔曼滤波误差补偿
    
    fprintf('  执行卡尔曼滤波误差补偿...\n');
    
    N = size(navigation_result.lat, 1);
    dt = 1/params.fs;
    
    % 状态向量: [δL, δλ, δh, δve, δvn, δvu, εx, εy, εz, ∇x, ∇y, ∇z]
    % 位置误差(3) + 速度误差(3) + 陀螺零偏(3) + 加速度计零偏(3)
    n_states = 12;
    
    % 初始化
    x = zeros(n_states, 1);          % 状态估计
    P = diag([1e-6, 1e-6, 1e-3, 1e-3, 1e-3, 1e-3, 1e-8, 1e-8, 1e-8, 1e-6, 1e-6, 1e-6]); % 协方差
    
    % 存储结果
    kf_states = zeros(N, n_states);
    kf_covariance = zeros(N, n_states);
    
    % 状态转移矩阵 F
    F = eye(n_states);
    
    % 观测矩阵 H (假设有GPS观测)
    H = zeros(6, n_states);
    H(1:3, 1:3) = eye(3);  % 位置观测
    H(4:6, 4:6) = eye(3);  % 速度观测
    
    % 过程噪声协方差 Q
    Q = params.kf_q;
    
    % 观测噪声协方差 R
    R = params.kf_r;
    
    % 卡尔曼滤波主循环
    for k = 2:N
        % 1. 状态预测
        % 提取导航参数
        lat = navigation_result.lat(k-1);
        ve = navigation_result.ve(k-1);
        vn = navigation_result.vn(k-1);
        vu = navigation_result.vu(k-1);
        
        % 计算状态转移矩阵 F
        F = update_state_transition_matrix(F, lat, ve, vn, vu, dt, params);
        
        % 状态预测
        x_pred = F * x;
        P_pred = F * P * F' + Q;
        
        % 2. 观测更新（假设有GPS观测）
        % 生成模拟GPS观测（实际中应从GPS接收机获取）
        z_gps = generate_gps_observation(navigation_result, k, params);
        
        % 观测残差
        z_pred = H * x_pred;
        residual = z_gps - z_pred;
        
        % 卡尔曼增益
        S = H * P_pred * H' + R;
        K = P_pred * H' / S;
        
        % 状态更新
        x = x_pred + K * residual;
        P = (eye(n_states) - K * H) * P_pred;
        
        % 3. 误差反馈校正
        navigation_result = apply_error_correction(navigation_result, x, k, params);
        
        % 存储结果
        kf_states(k, :) = x';
        kf_covariance(k, :) = diag(P)';
        
        % 重置陀螺零偏估计（缓慢变化）
        if mod(k, 1000) == 0
            x(7:9) = 0.99 * x(7:9);  % 遗忘因子
        end
    end
    
    fprintf('  卡尔曼滤波完成\n');
end

function F = update_state_transition_matrix(F, lat, ve, vn, vu, dt, params)
    % 更新状态转移矩阵
    
    % 地球参数
    omega_ie = params.omega_ie;
    Re = params.Re;
    e = params.e;
    
    % 地球半径
    Re_h = Re / sqrt(1 - e^2 * sin(lat)^2);
    Rn_h = Re * (1 - e^2) / (1 - e^2 * sin(lat)^2)^(3/2);
    
    % 位置误差到速度误差的转换
    F(1,4) = 1/(Rn_h + 0);      % ∂δve/∂δL
    F(2,5) = 1/((Re_h + 0) * cos(lat)); % ∂δvn/∂δλ
    F(3,6) = 1;                 % ∂δvu/∂δh
    
    % 速度误差到位置误差的转换
    F(4,1) = -ve * vn * tan(lat) / (Re_h + 0)^2;
    F(4,2) = -(ve^2 * tan(lat) + vn^2) / (Re_h + 0)^2;
    F(4,3) = ve * vn / (Re_h + 0)^2;
    
    F(5,1) = (ve^2 + vn^2) / (Re_h + 0)^2;
    F(5,2) = ve * vn / ((Re_h + 0)^2 * cos(lat)^2);
    F(5,3) = -vn / (Re_h + 0)^2;
    
    F(6,1) = 2 * omega_ie * cos(lat) * vu;
    F(6,2) = 2 * omega_ie * sin(lat) * vu;
    F(6,3) = -2 * omega_ie * (ve * cos(lat) + vn * sin(lat));
    
    % 陀螺零偏到姿态误差的转换
    F(7,7) = 1;  % 陀螺零偏缓慢变化
    F(8,8) = 1;
    F(9,9) = 1;
    
    % 加速度计零偏到速度误差的转换
    F(10,4) = 1;  % 加速度计零偏直接影响速度误差
    F(11,5) = 1;
    F(12,6) = 1;
    
    % 时间更新
    F = eye(12) + F * dt;
end

function z_gps = generate_gps_observation(navigation_result, k, params)
    % 生成模拟GPS观测
    
    % 真实位置（添加噪声）
    lat_true = navigation_result.lat(k);
    lon_true = navigation_result.lon(k);
    alt_true = navigation_result.alt(k);
    
    % GPS观测噪声
    pos_noise_std = [5, 5, 10];  % 位置噪声标准差 (m)
    
    z_gps = [lat_true + pos_noise_std(1)*randn(), ...
             lon_true + pos_noise_std(2)*randn(), ...
             alt_true + pos_noise_std(3)*randn(), ...
             navigation_result.ve(k) + 0.1*randn(), ...
             navigation_result.vn(k) + 0.1*randn(), ...
             navigation_result.vu(k) + 0.1*randn()];
end

function navigation_result = apply_error_correction(navigation_result, x, k, params)
    % 应用误差校正
    
    % 位置误差校正
    navigation_result.lat(k) = navigation_result.lat(k) - x(1);
    navigation_result.lon(k) = navigation_result.lon(k) - x(2);
    navigation_result.alt(k) = navigation_result.alt(k) - x(3);
    
    % 速度误差校正
    navigation_result.ve(k) = navigation_result.ve(k) - x(4);
    navigation_result.vn(k) = navigation_result.vn(k) - x(5);
    navigation_result.vu(k) = navigation_result.vu(k) - x(6);
    
    % 姿态误差校正（通过四元数修正）
    delta_q = [1, x(7)/2, x(8)/2, x(9)/2];  % 小角度近似
    q_corrected = quatmultiply(navigation_result.quaternion(k, :)', delta_q');
    navigation_result.quaternion(k, :) = q_corrected';
    
    % 更新欧拉角
    [pitch, roll, yaw] = quat2euler(q_corrected);
    navigation_result.pitch(k) = pitch;
    navigation_result.roll(k) = roll;
    navigation_result.yaw(k) = yaw;
end

2.7 参考轨迹生成：generate_reference_trajectory.m

function reference_trajectory = generate_reference_trajectory(params)
    % 生成参考轨迹（真值）
    
    N = params.fs * params.duration;
    dt = 1/params.fs;
    
    reference_trajectory = struct();
    
    % 生成理想轨迹
    lat = params.init_lat * ones(N, 1);
    lon = params.init_lon * ones(N, 1);
    alt = params.init_alt * ones(N, 1);
    ve = params.init_ve * ones(N, 1);
    vn = params.init_vn * ones(N, 1);
    vu = params.init_vu * ones(N, 1);
    pitch = params.init_pitch * ones(N, 1);
    roll = params.init_roll * ones(N, 1);
    yaw = params.init_yaw * ones(N, 1);
    
    % 添加机动
    for k = 2:N
        t = (k-1) * dt;
        
        % 速度变化
        ve(k) = params.init_ve + 5 * sin(0.1*t);
        vn(k) = params.init_vn + 3 * sin(0.05*t);
        vu(k) = params.init_vu + 0.5 * sin(0.2*t);
        
        % 姿态变化
        pitch(k) = params.init_pitch + 0.05 * sin(0.1*t);
        roll(k) = params.init_roll + 0.03 * sin(0.08*t);
        yaw(k) = params.init_yaw + 0.1 * sin(0.05*t);
        
        % 位置更新
        Re_h = params.Re / sqrt(1 - params.e^2 * sin(lat(k-1))^2);
        Rn_h = params.Re * (1 - params.e^2) / (1 - params.e^2 * sin(lat(k-1))^2)^(3/2);
        
        lat(k) = lat(k-1) + vn(k) * dt / (Rn_h + alt(k-1));
        lon(k) = lon(k-1) + ve(k) * dt / ((Re_h + alt(k-1)) * cos(lat(k-1)));
        alt(k) = alt(k-1) + vu(k) * dt;
    end
    
    reference_trajectory.lat = lat;
    reference_trajectory.lon = lon;
    reference_trajectory.alt = alt;
    reference_trajectory.ve = ve;
    reference_trajectory.vn = vn;
    reference_trajectory.vu = vu;
    reference_trajectory.pitch = pitch;
    reference_trajectory.roll = roll;
    reference_trajectory.yaw = yaw;
    reference_trajectory.time = (0:N-1)' * dt;
end

2.8 性能评估：evaluate_ins_performance.m

function evaluate_ins_performance(navigation_result, reference_trajectory, params)
    % 评估惯性导航性能
    
    fprintf('\n=== 惯性导航性能评估 ===\n');
    
    % 计算误差
    lat_error = (navigation_result.lat - reference_trajectory.lat) * 180/pi * 3600; % 秒
    lon_error = (navigation_result.lon - reference_trajectory.lon) * 180/pi * 3600; % 秒
    alt_error = navigation_result.alt - reference_trajectory.alt; % 米
    
    ve_error = navigation_result.ve - reference_trajectory.ve; % m/s
    vn_error = navigation_result.vn - reference_trajectory.vn; % m/s
    vu_error = navigation_result.vu - reference_trajectory.vu; % m/s
    
    pitch_error = (navigation_result.pitch - reference_trajectory.pitch) * 180/pi * 3600; % 角秒
    roll_error = (navigation_result.roll - reference_trajectory.roll) * 180/pi * 3600; % 角秒
    yaw_error = (navigation_result.yaw - reference_trajectory.yaw) * 180/pi * 3600; % 角秒
    
    % 统计指标
    fprintf('位置误差:\n');
    fprintf('  纬度误差: 均值=%.2f″, 标准差=%.2f″, 最大值=%.2f″\n', ...
            mean(lat_error), std(lat_error), max(abs(lat_error)));
    fprintf('  经度误差: 均值=%.2f″, 标准差=%.2f″, 最大值=%.2f″\n', ...
            mean(lon_error), std(lon_error), max(abs(lon_error)));
    fprintf('  高度误差: 均值=%.2f m, 标准差=%.2f m, 最大值=%.2f m\n', ...
            mean(alt_error), std(alt_error), max(abs(alt_error)));
    
    fprintf('\n速度误差:\n');
    fprintf('  东向速度: 均值=%.3f m/s, 标准差=%.3f m/s\n', mean(ve_error), std(ve_error));
    fprintf('  北向速度: 均值=%.3f m/s, 标准差=%.3f m/s\n', mean(vn_error), std(vn_error));
    fprintf('  天向速度: 均值=%.3f m/s, 标准差=%.3f m/s\n', mean(vu_error), std(vu_error));
    
    fprintf('\n姿态误差:\n');
    fprintf('  俯仰角: 均值=%.2f″, 标准差=%.2f″\n', mean(pitch_error), std(pitch_error));
    fprintf('  横滚角: 均值=%.2f″, 标准差=%.2f″\n', mean(roll_error), std(roll_error));
    fprintf('  航向角: 均值=%.2f″, 标准差=%.2f″\n', mean(yaw_error), std(yaw_error));
    
    % 计算漂移率
    duration_hours = params.duration / 3600;
    fprintf('\n漂移率:\n');
    fprintf('  位置漂移: %.2f m/h\n', mean(sqrt(lat_error.^2 + lon_error.^2)) / duration_hours);
    fprintf('  姿态漂移: %.2f ″/h\n', mean(sqrt(pitch_error.^2 + roll_error.^2 + yaw_error.^2)) / duration_hours);
    
    % 精度等级评定
    fprintf('\n精度等级评定:\n');
    if mean(sqrt(lat_error.^2 + lon_error.^2)) < 10
        fprintf('  ✓ 战术级精度 (<10m)\n');
    elseif mean(sqrt(lat_error.^2 + lon_error.^2)) < 100
        fprintf('  ○ 导航级精度 (<100m)\n');
    else
        fprintf('  ⚠ 消费级精度 (>100m)\n');
    end
end

2.9 结果可视化：visualize_ins_results.m

function visualize_ins_results(navigation_result, reference_trajectory, kf_states, params)
    % 可视化惯性导航结果
    
    figure('Name', '惯性导航精解算结果', 'Color', 'white', 'Position', [100, 100, 1600, 900]);
    
    % 1. 三维轨迹
    subplot(3,4,1);
    plot3(reference_trajectory.lon*180/pi, reference_trajectory.lat*180/pi, reference_trajectory.alt, ...
          'g-', 'LineWidth', 2, 'DisplayName', '真值');
    hold on;
    plot3(navigation_result.lon*180/pi, navigation_result.lat*180/pi, navigation_result.alt, ...
          'r--', 'LineWidth', 1.5, 'DisplayName', 'INS解算');
    xlabel('经度 (°)'); ylabel('纬度 (°)'); zlabel('高度 (m)');
    title('三维导航轨迹');
    legend('Location', 'best');
    grid on;
    axis equal;
    
    % 2. 位置误差随时间变化
    subplot(3,4,2);
    t = navigation_result.time;
    lat_error = (navigation_result.lat - reference_trajectory.lat) * 180/pi * 3600;
    lon_error = (navigation_result.lon - reference_trajectory.lon) * 180/pi * 3600;
    alt_error = navigation_result.alt - reference_trajectory.alt;
    
    plot(t, lat_error, 'r-', 'LineWidth', 1.5, 'DisplayName', '纬度误差');
    hold on;
    plot(t, lon_error, 'b-', 'LineWidth', 1.5, 'DisplayName', '经度误差');
    plot(t, alt_error, 'g-', 'LineWidth', 1.5, 'DisplayName', '高度误差');
    xlabel('时间 (s)'); ylabel('误差');
    title('位置误差随时间变化');
    legend('Location', 'best');
    grid on;
    
    % 3. 速度误差
    subplot(3,4,3);
    ve_error = navigation_result.ve - reference_trajectory.ve;
    vn_error = navigation_result.vn - reference_trajectory.vn;
    vu_error = navigation_result.vu - reference_trajectory.vu;
    
    plot(t, ve_error, 'r-', 'LineWidth', 1.5, 'DisplayName', '东向速度误差');
    hold on;
    plot(t, vn_error, 'b-', 'LineWidth', 1.5, 'DisplayName', '北向速度误差');
    plot(t, vu_error, 'g-', 'LineWidth', 1.5, 'DisplayName', '天向速度误差');
    xlabel('时间 (s)'); ylabel('速度误差 (m/s)');
    title('速度误差随时间变化');
    legend('Location', 'best');
    grid on;
    
    % 4. 姿态误差
    subplot(3,4,4);
    pitch_error = (navigation_result.pitch - reference_trajectory.pitch) * 180/pi * 3600;
    roll_error = (navigation_result.roll - reference_trajectory.roll) * 180/pi * 3600;
    yaw_error = (navigation_result.yaw - reference_trajectory.yaw) * 180/pi * 3600;
    
    plot(t, pitch_error, 'r-', 'LineWidth', 1.5, 'DisplayName', '俯仰误差');
    hold on;
    plot(t, roll_error, 'b-', 'LineWidth', 1.5, 'DisplayName', '横滚误差');
    plot(t, yaw_error, 'g-', 'LineWidth', 1.5, 'DisplayName', '航向误差');
    xlabel('时间 (s)'); ylabel('姿态误差 (″)');
    title('姿态误差随时间变化');
    legend('Location', 'best');
    grid on;
    
    % 5. 卡尔曼滤波状态估计
    subplot(3,4,5);
    plot(t, kf_states(:,1)*180/pi*3600, 'r-', 'LineWidth', 1.5, 'DisplayName', '纬度误差估计');
    hold on;
    plot(t, kf_states(:,2)*180/pi*3600, 'b-', 'LineWidth', 1.5, 'DisplayName', '经度误差估计');
    plot(t, kf_states(:,3), 'g-', 'LineWidth', 1.5, 'DisplayName', '高度误差估计');
    xlabel('时间 (s)'); ylabel('估计误差');
    title('卡尔曼滤波状态估计');
    legend('Location', 'best');
    grid on;
    
    % 6. 卡尔曼滤波协方差
    subplot(3,4,6);
    plot(t, kf_states(:,7)*180/pi*3600, 'r-', 'LineWidth', 1.5, 'DisplayName', '俯仰零偏估计');
    hold on;
    plot(t, kf_states(:,8)*180/pi*3600, 'b-', 'LineWidth', 1.5, 'DisplayName', '横滚零偏估计');
    plot(t, kf_states(:,9)*180/pi*3600, 'g-', 'LineWidth', 1.5, 'DisplayName', '航向零偏估计');
    xlabel('时间 (s)'); ylabel('零偏估计 (″)');
    title('陀螺零偏估计');
    legend('Location', 'best');
    grid on;
    
    % 7. 轨迹平面图
    subplot(3,4,7);
    plot(reference_trajectory.lon*180/pi, reference_trajectory.lat*180/pi, 'g-', 'LineWidth', 2, 'DisplayName', '真值');
    hold on;
    plot(navigation_result.lon*180/pi, navigation_result.lat*180/pi, 'r--', 'LineWidth', 1.5, 'DisplayName', 'INS解算');
    xlabel('经度 (°)'); ylabel('纬度 (°)');
    title('轨迹平面图');
    legend('Location', 'best');
    grid on;
    axis equal;
    
    % 8. 高度剖面
    subplot(3,4,8);
    plot(t, reference_trajectory.alt, 'g-', 'LineWidth', 2, 'DisplayName', '真值');
    hold on;
    plot(t, navigation_result.alt, 'r--', 'LineWidth', 1.5, 'DisplayName', 'INS解算');
    xlabel('时间 (s)'); ylabel('高度 (m)');
    title('高度剖面');
    legend('Location', 'best');
    grid on;
    
    % 9. 速度剖面
    subplot(3,4,9);
    plot(t, reference_trajectory.ve, 'r-', 'LineWidth', 1.5, 'DisplayName', '东向速度真值');
    hold on;
    plot(t, navigation_result.ve, 'r--', 'LineWidth', 1, 'DisplayName', '东向速度INS');
    plot(t, reference_trajectory.vn, 'b-', 'LineWidth', 1.5, 'DisplayName', '北向速度真值');
    plot(t, navigation_result.vn, 'b--', 'LineWidth', 1, 'DisplayName', '北向速度INS');
    xlabel('时间 (s)'); ylabel('速度 (m/s)');
    title('速度剖面');
    legend('Location', 'best');
    grid on;
    
    % 10. 姿态剖面
    subplot(3,4,10);
    plot(t, reference_trajectory.pitch*180/pi, 'r-', 'LineWidth', 1.5, 'DisplayName', '俯仰角真值');
    hold on;
    plot(t, navigation_result.pitch*180/pi, 'r--', 'LineWidth', 1, 'DisplayName', '俯仰角INS');
    plot(t, reference_trajectory.roll*180/pi, 'b-', 'LineWidth', 1.5, 'DisplayName', '横滚角真值');
    plot(t, navigation_result.roll*180/pi, 'b--', 'LineWidth', 1, 'DisplayName', '横滚角INS');
    xlabel('时间 (s)'); ylabel('角度 (°)');
    title('姿态剖面');
    legend('Location', 'best');
    grid on;
    
    % 11. 误差统计直方图
    subplot(3,4,11);
    histogram(lat_error, 50, 'FaceColor', 'red', 'EdgeColor', 'black');
    xlabel('纬度误差 (″)'); ylabel('频数');
    title('纬度误差分布');
    grid on;
    
    % 12. 系统参数表
    subplot(3,4,12); axis off;
    param_text = {
        sprintf('导航坐标系: %s', params.nav_frame)
        sprintf('地球模型: %s', params.earth_model)
        sprintf('采样频率: %.1f Hz', params.fs)
        sprintf('总时长: %.1f s', params.duration)
        sprintf('初始纬度: %.6f°', params.init_lat*180/pi)
        sprintf('初始经度: %.6f°', params.init_lon*180/pi)
        sprintf('陀螺零偏: %.2f °/h', params.gyro_bias*180/pi*3600)
        sprintf('加速度计零偏: %.1f μg', params.accel_bias/9.8*1e6)
    };
    text(0.1, 0.9, '惯性导航系统参数:', 'FontSize', 12, 'FontWeight', 'bold');
    for i = 1:length(param_text)
        text(0.1, 0.9 - i*0.1, param_text{i}, 'FontSize', 10);
    end
    title('系统配置信息');
end

参考代码 惯性导航精解算程序 www.youwenfan.com/contentcsu/60173.html

三、扩展功能模块

3.1 GNSS/INS组合导航

function gnss_ins_integration()
    % GNSS/INS松组合导航
    
    % 1. 数据预处理
    %    - IMU数据去噪
    %    - GNSS周跳探测与修复
    %    - 时间同步
    
    % 2. 松组合卡尔曼滤波
    %    - 状态向量: INS误差状态 + GNSS误差状态
    %    - 观测向量: GNSS位置/速度 - INS位置/速度
    
    % 3. 紧组合（可选）
    %    - 伪距/载波相位观测
    %    - 整周模糊度固定
    
    % 4. 完好性监测
    %    - RAIM/FDE
    %    - 故障检测与隔离
end

3.2 车载/机载专用模块

function vehicle_specific_ins()
    % 车载专用惯性导航
    
    % 1. 车辆运动约束
    %    - 非完整性约束 (NHC): 侧向和垂向速度为0
    %    - 零速更新 (ZUPT): 停车时速度归零
    
    % 2. 里程计辅助
    %    - 轮速脉冲计数
    %    - 转向角传感器
    
    % 3. 视觉辅助
    %    - 视觉里程计
    %    - 特征点跟踪
end