SINS惯性导航系统基础构建与应用技术详解

息相吹

1139人浏览 · 2025-10-19 13:59:10

息相吹 · 2025-10-19 13:59:10 发布

简介：该资料为Matveev V.V.与Raspopov V.YA.合著的《SINS基础构造》技术书籍，深入讲解卫星惯性导航系统（SINS）的设计原理与实现方法。内容涵盖惯性传感器工作原理、系统架构、误差补偿、数据融合算法（如卡尔曼滤波）、软件设计及实际应用场景，适用于航空、航海、无人系统等领域。本书为工程技术人员和研究人员提供了完整的SINS理论基础与实践指导，是掌握高精度自主导航技术的重要学习资源。
INS SINS_pdf_SINS_

1. 惯性导航系统（INS）基本原理

惯性导航系统（INS）基于牛顿力学原理，通过测量载体在自身坐标系下的加速度与角速度，经时间积分运算解算出位置、速度和姿态信息。其核心依赖于惯性测量单元（IMU）中的加速度计和陀螺仪，分别采集三轴线加速度和角速率数据。为实现精确导航，需建立惯性系（i）、地理系（n）与载体系（b）之间的转换关系，通常采用方向余弦矩阵、四元数或欧拉角描述姿态变换。其中，四元数因避免万向节死锁且计算高效，广泛应用于捷联式惯导（SINS）的姿态更新算法中。

导航方程在地理系下构建，包含速度、位置及姿态微分方程：

\begin{aligned}
\dot{\mathbf{v}}^n &= \mathbf{C} b^n \mathbf{f}^b - (2\boldsymbol{\omega} {ie}^n + \boldsymbol{\omega}_{en}^n) \times \mathbf{v}^n + \mathbf{g}^n \
\dot{\mathbf{r}} &= \mathbf{R}^{-1}(\mathbf{v}^n)
\end{aligned}

式中 $\mathbf{C} b^n$ 为载体系到地理系的旋转矩阵，$\mathbf{f}^b$ 为比力输出，$\boldsymbol{\omega} {ie}^n$ 为地球自转角速度，$\mathbf{g}^n$ 为当地重力矢量。

捷联式INS将IMU直接固定于载体，通过数学平台替代物理稳定平台，相较平台式系统结构紧凑、成本低，已成为现代主流方案。本章所建立的理论框架为后续SINS系统设计与误差补偿提供基础支撑。

2. 卫星惯性导航系统（SINS）架构与组成

卫星惯性导航系统（Satellite-Inertial Navigation System, SINS）是一种将全球导航卫星系统（GNSS）与惯性测量单元（IMU）深度融合的高精度、高可靠性导航解决方案。其核心优势在于结合了GNSS长期稳定性与IMU短时高动态响应能力，通过多源信息融合算法实现全天候、全地形下的连续、精准定位与姿态解算。在现代航空航天、无人驾驶、智能交通及军事装备中，SINS已成为关键核心技术之一。

2.1 SINS系统的整体架构设计

2.1.1 系统功能模块划分：IMU、GNSS接收机、主控单元与通信接口

SINS系统由多个功能模块协同工作构成，各模块之间通过精确的时间同步与数据交互机制保障系统整体性能。主要功能模块包括：

惯性测量单元（IMU） ：负责采集载体三轴加速度和角速度信号，是系统的核心传感器之一。
GNSS接收机 ：提供绝对位置、速度和时间（PVT）信息，作为外部参考基准用于校正IMU累积误差。
主控单元（MCU/FPGA/SoC） ：执行导航解算、滤波融合、状态估计等核心算法，通常运行于实时操作系统之上。
通信接口模块 ：实现内部模块间以及与外部设备的数据交换，常见协议包括CAN、UART、SPI、Ethernet、RS422等。

下表为典型SINS系统各模块的功能特性对比：

模块	功能描述	数据输出频率	典型延迟	关键性能指标
IMU	测量线加速度与角速率	100–1000 Hz	<1ms	零偏稳定性、噪声密度、带宽
GNSS接收机	提供经纬度、高度、速度、UTC时间	1–20 Hz	10–100ms	定位精度、PDOP、首次定位时间
主控单元	执行EKF融合、姿态解算、误差补偿	实时处理	取决于算法复杂度	计算吞吐量、内存带宽
通信接口	数据传输与指令下发	可配置	协议相关	波特率、误码率、抗干扰能力

从上表可见，IMU具有高频低延迟特点，适合捕捉快速运动变化；而GNSS更新频率较低但具备全局无漂移特性，二者互补性强。

数据流驱动的模块协作模式

SINS系统以数据流为中心组织各模块运行。IMU持续以高采样率输出原始加速度与角速度数据，经温度补偿与标定后送入主控单元进行姿态更新；GNSS接收机周期性输出PVT结果，通过串行或网络接口传入主控系统；主控单元利用扩展卡尔曼滤波器（EKF）对两类数据进行融合，输出最优估计的位置、速度、姿态四元数及传感器误差状态。

flowchart TD
    A[IMU] -->|Raw Accel & Gyro @ 500Hz| B(Preprocessing)
    C[GNSS Receiver] -->|PVT @ 10Hz| D(Data Alignment & Time Sync)
    B --> E[Navigation Engine]
    D --> E
    E --> F[EKF Filter Core]
    F --> G[State Estimation Output]
    G --> H[Position/Vel/Orientation]
    G --> I[Error States: Bias, Scale Factor]
    H --> J[External Systems]
    I --> K[Feedback to IMU Model]

该流程图清晰展示了SINS中各模块之间的数据流向与控制逻辑。值得注意的是， 时间同步 在整个架构中极为关键——若IMU与GNSS数据未在统一时钟基准下对齐，将导致滤波发散或精度下降。因此，在硬件层面常采用PPS（秒脉冲）信号同步GNSS时间戳，并通过插值算法将GNSS观测映射到IMU时间网格。

2.1.2 数据流拓扑结构与实时性要求分析

SINS系统对数据流拓扑结构的设计直接影响其导航精度与鲁棒性。典型的拓扑结构可分为 集中式 与 分布式 两种。

集中式架构（Centralized Topology）

所有传感器直接连接至主控处理器，由单一计算节点完成全部数据采集、预处理与融合运算。优点是控制逻辑集中、调试方便；缺点是对主处理器性能要求高，存在单点故障风险。

分布式架构（Distributed Topology）

IMU与GNSS分别配备本地微控制器，先进行初步处理（如温补、坐标变换），再通过高速总线上传至主控单元。此结构可降低主CPU负载，提升系统可扩展性，适用于多IMU或多GNSS天线场景。

架构类型	延迟表现	扩展性	实现难度	适用场景
集中式	低（<5ms）	中等	低	小型无人机、车载终端
分布式	中（5–15ms）	高	高	航空航天、舰载平台

在实时性方面，SINS需满足严格的硬实时约束。例如，在飞行器应用中，姿态更新周期不得超过2ms（即500Hz），否则会导致控制系统响应滞后。为此，系统常采用中断驱动方式采集IMU数据：

// 示例：基于STM32 HAL库的IMU定时中断采集
void TIM3_IRQHandler(void) {
    if (__HAL_TIM_GET_FLAG(&htim3, TIM_FLAG_UPDATE)) {
        HAL_TIM_IRQHandler(&htim3);
        // 触发IMU SPI读取任务
        read_imu_data();  // 包含accel_x, accel_y, accel_z, gyro_x等
        // 将数据压入环形缓冲区
        push_to_fifo(imu_buffer, &current_imu_sample);
        // 标记新数据可用，唤醒导航线程
        xSemaphoreGiveFromISR(imu_data_ready_sem, NULL);
    }
}

代码逻辑逐行解读：

TIM3_IRQHandler ：定时器中断服务函数，每2ms触发一次。
__HAL_TIM_GET_FLAG ：检查是否为更新中断，防止误触发。
read_imu_data() ：调用SPI接口从IMU芯片（如ADIS16470或BMI088）读取最新传感器数据。
push_to_fifo ：将数据存入环形缓冲区，避免丢失。
xSemaphoreGiveFromISR ：释放二值信号量，通知RTOS中的导航任务处理新数据。

上述机制确保了IMU数据采集的确定性与低抖动，是实现高精度导航的前提。

此外，为了应对突发负载（如GNSS信号丢失后的纯惯导推算），系统还需设计合理的任务优先级调度策略。典型RTOS任务划分如下：

任务名称	优先级	周期	功能说明
IMU采集	最高（P0）	2ms	实时获取惯性数据
导航解算	高（P1）	10ms	姿态更新、速度积分
EKF融合	高（P1）	100ms	引入GNSS观测进行状态修正
通信输出	中（P2）	50ms	向外部发送NMEA或自定义格式数据
自检与监控	低（P3）	1s	故障诊断、健康状态上报

通过这种分层任务调度模型，系统能够在资源受限条件下保持稳定运行，同时满足不同模块的实时性需求。

2.2 核心硬件组件集成方案

2.2.1 惯性测量单元（IMU）与全球导航卫星系统（GNSS）模块的物理集成

硬件集成是SINS系统可靠性的基础。IMU与GNSS模块的物理布局需综合考虑电气兼容性、机械刚性、热稳定性与电磁屏蔽等多个因素。

机械安装要求

IMU应尽可能靠近载体质心安装，以减少杠杆臂效应引起的角运动耦合误差。GNSS天线则需布置在顶部开阔区域，避免金属遮挡。两者之间的相对位置需精确标定（称为“杆臂矢量”），并在滤波器中予以补偿。

杆臂误差模型如下：
\vec{r}_{err} = \vec{\omega} \times \vec{l}
其中 $\vec{l}$ 为IMU到GNSS天线相位中心的偏移向量，$\vec{\omega}$ 为角速度。若不补偿，该误差将在高速旋转时显著影响融合精度。

实际工程中，常用铝制或碳纤维支架固定两模块，确保结构刚度大于1kHz共振频率，防止振动引入虚假加速度。

电气集成设计

IMU与GNSS通常采用不同供电域。IMU对电源纹波极为敏感，建议使用LDO稳压+π型滤波电路；GNSS则需保证足够的瞬态电流供应能力，以防搜星失败。

典型供电设计方案：

+5V输入
├───[LC滤波]───→ LM1117-3.3V (LDO) ───→ IMU (低噪声)
└───[钽电容+磁珠]───→ AMS1117-3.3V ───→ GNSS模块

同时，数字地与模拟地应分开布线，最后单点汇接，减少共模干扰。

2.2.2 主处理器选型与嵌入式系统平台搭建

主处理器是SINS系统的“大脑”，其选型需权衡计算能力、功耗、接口资源与开发支持。

处理器类型对比

类型	代表型号	浮点性能	内存接口	适用场景
MCU	STM32H7	~1GHz, FPU	SDRAM扩展	成本敏感型应用
SoC	NXP i.MX8M Plus	Cortex-A53 + NPU	DDR4	AI增强融合
FPGA	Xilinx Zynq UltraScale+ MPSoC	可编程逻辑并行处理	LPDDR4	超低延迟系统

对于高端SINS系统，Zynq系列FPGA因其兼具ARM处理器与可编程逻辑单元，成为理想选择。可在PL端实现IMU数据采集流水线，在PS端运行Linux+EKF融合程序。

嵌入式平台构建示例（基于Yocto Project）

# 创建项目环境
bitbake-layers create-layer meta-sins
cd meta-sins && recipes-kernel/linux/

定制内核配置以启用高精度定时器与PREEMPT_RT补丁：

CONFIG_HIGH_RES_TIMERS=y
CONFIG_PREEMPT_RT_FULL=y
CONFIG_CPU_FREQ_GOV_PERFORMANCE=y

编译生成最小化根文件系统镜像，仅包含必要服务：

IMAGE_INSTALL += " \
    kernel-modules \
    openssh-sftp-server \
    chrony \
    gpsd \
    libeigen3-dev \
    gcc-arm-none-eabi \
"

最终生成的系统可在100ms内完成启动，并进入导航待命状态，满足车载快速启动需求。

2.3 软件系统分层架构

2.3.1 实时操作系统（RTOS）在SINS中的应用

RTOS是保障SINS确定性行为的关键。主流选择包括FreeRTOS、RT-Thread、VxWorks与QNX。

以FreeRTOS为例，其轻量级内核（<10KB）适合资源受限设备。通过创建多个任务实现模块化解耦：

void nav_task(void *pvParameters) {
    TickType_t xLastWakeTime = xTaskGetTickCount();
    const TickType_t xFrequency = pdMS_TO_TICKS(10); // 100Hz

    while(1) {
        vTaskDelayUntil(&xLastWakeTime, xFrequency);
        update_attitude_from_gyro();   // 四元数更新
        integrate_velocity_position(); // 速度/位置积分
        apply_gravity_compensation();  // 重力补偿
    }
}

// 创建任务
xTaskCreate(nav_task, "NavEngine", 1024, NULL, tskIDLE_PRIORITY + 3, NULL);

参数说明：
- vTaskDelayUntil ：确保周期严格为10ms，消除累积误差。
- tskIDLE_PRIORITY + 3 ：设置较高优先级，避免被低优先级任务阻塞。

2.3.2 导航解算、数据融合与状态输出软件模块设计

软件模块采用分层架构：

graph TD
    A[Sensors] --> B[Driver Layer]
    B --> C[Middleware: FIFO, Timestamping]
    C --> D[Algorithm Layer]
    D --> E[EKF Fusion Core]
    E --> F[Output Formatter]
    F --> G[NMEA-0183 / Custom Binary]

导航解算部分采用四元数法更新姿态：

void update_orientation(float gx, float gy, gz, float dt) {
    float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];
    float wx = gx - gyro_bias[0];
    float wy = gy - gyro_bias[1];
    float wz = gz - gyro_bias[2];

    // 四元数微分方程
    float dq1 = 0.5f * (-q2*wx - q3*wy - q4*wz);
    float dq2 = 0.5f * ( q1*wx - q4*wy + q3*wz);
    float dq3 = 0.5f * ( q4*wx + q1*wy - q2*wz);
    float dq4 = 0.5f * (-q3*wx + q2*wy + q1*wz);

    q[0] += dq1 * dt;
    q[1] += dq2 * dt;
    q[2] += dq3 * dt;
    q[3] += dq4 * dt;

    normalize_quaternion(q); // 单位化
}

逻辑分析：
- 使用陀螺仪角速度减去已知零偏后作为输入。
- 根据四元数微分方程计算增量。
- 积分后归一化，防止数值漂移。

该算法每2ms执行一次，构成姿态更新外环的基础。

2.4 多源传感器协同工作机制

2.4.1 GNSS/INS松耦合与紧耦合模式对比

特性	松耦合	紧耦合
融合层级	PVT级	原始观测量级
抗遮挡能力	弱	强
实现复杂度	低	高
对GNSS依赖	高	低

在城市峡谷环境中，紧耦合可通过伪距残差辅助INS，即使仅有3颗卫星仍可维持定位。

2.4.2 辅助传感器（如磁力计、气压计）引入策略

磁力计用于航向角补充（尤其在静态或低速时），气压计提供高程参考。其观测方程可加入EKF作为额外更新项：

z_{baro} = h_{true} + v_b \
z_{mag} = \arctan2(B_y, B_x) + v_m

需注意磁场干扰建模与温度补偿，否则会误导航向估计。

综上所述，SINS系统是一个高度集成的软硬件协同体系，其架构设计需兼顾性能、可靠性与可维护性。唯有在每一个层级都做到精细优化，才能实现在复杂环境下的稳健导航能力。

3. 加速度计与陀螺仪工作原理及IMU设计实践

惯性测量单元（Inertial Measurement Unit, IMU）作为卫星惯性导航系统（SINS）的核心感知部件，其性能直接决定了整个系统的定位、定姿精度。IMU通常由三轴加速度计和三轴陀螺仪构成，分别用于测量载体在自身坐标系下的线加速度与角速度。这些原始数据经过积分运算可推导出速度、位置以及姿态信息，是实现自主导航的关键输入源。然而，不同类型的传感器在物理机理、制造工艺、环境适应性和长期稳定性方面存在显著差异，因此深入理解加速度计与陀螺仪的工作原理，并结合实际工程需求进行IMU的设计优化，是提升SINS整体性能的基础环节。

本章将从微观机理出发，系统剖析主流加速度计与陀螺仪的技术路线，重点比较各类器件的优劣特性及其适用场景；进而聚焦于IMU硬件集成过程中的关键挑战——包括机械安装误差控制、温度漂移补偿机制设计等；最后通过完整的标定流程与实验验证手段，展示如何将理论模型转化为高可靠性的工程产品。这一递进式结构不仅覆盖了从元器件选型到系统级测试的全链条技术路径，也为后续误差建模与滤波融合提供了精确的先验参数支持。

3.1 加速度计的工作机理与类型分析

加速度计是一种能够感知物体沿特定方向线性加速度变化的微机电或精密机械装置。其基本物理依据来源于牛顿第二定律 $ F = ma $，即当质量块受到外部加速度作用时会产生相应的惯性力，该力可通过敏感元件转换为电信号输出。根据信号转换方式的不同，当前主流加速度计主要分为电容式、压阻式和伺服式三大类，每种类型在灵敏度、动态范围、功耗及成本等方面各具优势，适用于不同的应用场景。

3.1.1 电容式、压阻式与伺服式加速度计原理比较

电容式加速度计 基于可变电容器结构设计。其核心由一个悬挂在弹性梁上的质量块（proof mass）和两个固定电极组成，形成差分电容对。当外界加速度施加于传感器时，质量块发生位移，导致两侧电容值发生变化。通过测量电容差值即可反推出加速度大小。该类传感器具有高分辨率、低噪声、良好的长期稳定性和较低的温度敏感性，广泛应用于中高端MEMS IMU中。

graph TD
    A[外部加速度输入] --> B[质量块位移]
    B --> C[差分电容变化]
    C --> D[电容-电压转换电路]
    D --> E[数字信号处理]
    E --> F[加速度输出]

上述流程图展示了电容式加速度计的典型信号链路。值得注意的是，由于电容变化量极小（常在飞法拉级别），必须采用高精度的电容检测电路（如Σ-Δ调制器）来提取有效信号。此外，非线性校正算法常被嵌入后端处理模块以提高测量精度。

相比之下， 压阻式加速度计 利用半导体材料的压阻效应实现力-电转换。当质量块因加速度产生应力时，集成在悬臂梁上的压敏电阻阻值随之改变，通过惠斯通电桥电路将其转化为电压信号输出。这类传感器响应速度快、带宽高，适合振动监测和冲击检测场合。但其缺点在于温度漂移较大，且易受封装应力影响，需配合复杂的温度补偿算法使用。

// 压阻式加速度计信号采集示例代码（基于ADC读取电桥输出）
float read_acceleration(void) {
    int adc_raw = ADC_Read(CHANNEL_0);           // 读取ADC原始值
    float v_bridge = (adc_raw * VREF) / ADC_MAX; // 转换为桥电压
    float accel = (v_bridge - V_ZERO) * SCALE_FACTOR; // 标定后得到加速度(m/s²)
    return accel;
}

代码逻辑逐行解读：
- 第2行：调用ADC驱动函数获取模拟电桥输出的数字化采样值；
- 第3行：将数字值按参考电压$V_{REF}$和最大码值$ADC_MAX$归一化为真实电压；
- 第4行：减去零点偏移$V_{ZERO}$并乘以标定比例因子$SCALE_FACTOR$，完成从电压到加速度的线性映射；
- 参数说明： VREF 为ADC参考电压（如3.3V）， ADC_MAX 为满量程值（如4095对应12位ADC）， SCALE_FACTOR 单位为$m/s^2/V$，需通过实验室标定获得。

第三类—— 伺服式（力平衡式）加速度计 ，则属于高精度惯性级设备，常见于航空、航天平台。其工作原理是在检测到质量块位移后，立即施加反馈电流生成电磁力使其恢复原位，从而保持“零位”状态。此时反馈电流与输入加速度成正比，具有极高的线性度和稳定性。此类传感器零偏稳定性可达μg级以下，但结构复杂、成本高昂、功耗大，一般不用于消费级或车载应用。

类型	精度等级	动态范围	温漂特性	典型应用场景
电容式	中高	±2g ~ ±50g	较低	无人机、机器人、GNSS辅助导航
压阻式	中	±50g ~ ±2000g	高	汽车碰撞检测、工业振动监控
伺服式	极高（惯性级）	±1g ~ ±5g	极低	军用飞机、战略导弹、潜艇导航

从表中可见，不同类型加速度计的选择需权衡精度、量程与环境适应性。例如，在需要长时间连续导航的任务中（如无人潜航器），优先选择低零偏漂移的电容式或伺服式器件；而在短时高强度动态环境下（如弹载记录仪），压阻式因其抗过载能力强而更具优势。

进一步地，现代高性能IMU往往采用多芯片异构集成策略，即在同一封装内集成多种类型的加速度计，借助数据融合算法实现宽动态范围与高精度兼顾。例如，主通道使用低噪声电容式传感器进行精细运动捕捉，辅通道配置高压力量程压阻式器件以防饱和，控制系统根据实时工况自动切换或加权融合输出结果。

综上所述，加速度计的选型不仅是硬件层面的决策，更涉及系统级的信号处理架构设计。只有充分理解其内在工作机制与性能边界，才能在复杂多变的实际环境中保障导航解算的鲁棒性与连续性。

3.2 陀螺仪技术发展路径

陀螺仪用于测量载体绕某一轴的角速度，是确定姿态变化率的核心元件。自机械转子陀螺问世以来，陀螺技术经历了从宏观机械系统向固态微结构的重大演进。目前主流陀螺仪主要包括光纤陀螺（FOG）、激光陀螺（RLG）和微机电系统（MEMS）陀螺三大类别，各自依托不同的物理原理实现角速度检测，在精度、体积、成本和可靠性方面呈现出明显的梯度分布。

3.2.1 光纤陀螺（FOG）、激光陀螺（RLG）与MEMS陀螺工作原理

光纤陀螺（Fiber Optic Gyro, FOG） 基于萨格纳克效应（Sagnac Effect）。当两束光沿同一闭合光路反向传播时，若系统整体发生旋转，则顺旋方向的光程变长，逆旋方向变短，造成相位差。该相位差与旋转角速度成正比，通过干涉仪检测即可解算角速率。FOG无活动部件，寿命长，启动快，精度可达0.001°/hr量级，广泛应用于中高精度战术级导航系统。

flowchart LR
    LightSource[光源] --> Splitter[分束器]
    Splitter --> CW[顺时针光路]
    Splitter --> CCW[逆时针光路]
    CW --> Coil[光纤环圈]
    CCW --> Coil
    Coil --> Detector[光电探测器]
    Detector --> SignalProcessor[信号处理器]
    SignalProcessor --> AngularRate[角速度输出]

该流程图清晰呈现了FOG的基本光学路径。其中，光纤环圈长度直接影响灵敏度——越长则相位差越大，但也会增加温度敏感性和制造难度。因此，高端FOG常配备温度均衡绕制工艺与闭环调制解调电路，以抑制环境扰动带来的误差。

激光陀螺（Ring Laser Gyro, RLG） 同样基于萨格纳克效应，但使用封闭谐振腔内的激光束代替光纤中的传导光。两束激光在三角形或四边形腔体内往返振荡，旋转引起的频率差（而非相位差）通过拍频技术测量。RLG具有极高的长期稳定性与抗冲击能力，曾广泛用于民航客机和军用飞机惯导系统。然而其存在“闭锁效应”（lock-in）问题——在极低转速下无法分辨方向，需引入机械抖动机构克服，增加了复杂性与故障率。

相较之下， MEMS陀螺仪 采用科里奥利力原理实现角速度测量。其内部包含一个持续振动的质量块（驱动模态），当载体发生旋转时，科里奥利力会激发垂直方向上的次级振动（感测模态），该位移通过电容或压阻方式检测并转换为电信号。MEMS陀螺体积小、成本低、易于批量生产，已成为消费电子和自动驾驶领域的标配组件。

# MEMS陀螺仪角速度解算伪代码
def calculate_angular_rate(capacitance_diff):
    delta_c = capacitance_diff                      # 感测电容差值
    voltage = gain_amplifier(delta_c)               # 放大电路增益放大
    filtered_v = low_pass_filter(voltage, fc=100)   # 滤除高频噪声
    omega = (filtered_v - bias) * scale_factor      # 标定后角速度(rad/s)
    return omega

代码逻辑逐行解读：
- 第2行：获取由科里奥利效应引起的电容变化量；
- 第3行：经前置放大器提升信噪比，增益系数需现场校准；
- 第4行：采用截止频率100Hz的低通滤波器去除机械共振干扰；
- 第5行：扣除静态零偏 bias 后乘以比例因子 scale_factor ，完成物理量转换；
- 参数说明： bias 随温度变化，需实时更新； scale_factor 单位为rad/s/V，依赖于驱动振幅稳定性。

尽管MEMS陀螺在性价比上占据绝对优势，但其角随机游走（ARW）和零偏不稳定性仍远逊于FOG与RLG。为此，近年来出现了混合架构趋势：例如将FOG与MEMS组合，前者提供基准姿态参考，后者承担高频动态补偿，通过卡尔曼滤波实现性能互补。

陀螺类型	零偏稳定性	角随机游走	启动时间	成本水平	典型应用领域
MEMS	10~100 °/hr	0.1~1 °/√hr	<1s	低	手机、无人机、ADAS
FOG	0.01~1 °/hr	0.001~0.01 °/√hr	~5s	中高	船舶、地面车辆、制导系统
RLG	0.001~0.1 °/hr	0.0005~0.005 °/√hr	~30s	高	商用飞机、战略平台

由此可见，陀螺仪的技术选型本质上是对精度、响应速度与经济性的综合权衡。随着硅基光子学与量子传感技术的发展，下一代陀螺或将突破现有物理极限，开启全新维度的惯性测量时代。

3.3 惯性测量单元（IMU）的设计实践

IMU的设计不仅仅是传感器的简单堆叠，而是涉及机械布局、电气接口、热管理与信号完整性的系统工程。一个高性能IMU必须确保三轴加速度计与陀螺仪严格正交安装，最小化交叉轴灵敏度，并在整个工作温区内维持稳定的输出特性。

3.3.1 三轴正交安装误差校准方法

理想情况下，三个加速度计应相互垂直并共原点。但由于加工公差，实际存在轴间夹角偏差（misalignment）和尺度因子不一致。设真实测量值为$\mathbf{a} {meas}$，理想输出为$\mathbf{a} {true}$，则二者关系可表示为：

\mathbf{a} {meas} = \mathbf{M} \cdot \mathbf{a} {true} + \mathbf{b}_a

其中$\mathbf{M}$为包含比例因子与非正交误差的3×3矩阵，$\mathbf{b}_a$为零偏向量。通过多位置静态翻转实验（如六面法），采集重力在各轴上的投影，建立超定方程组求解$\mathbf{M}$与$\mathbf{b}_a$，即可完成标定。

具体步骤如下：
1. 将IMU依次静置于六个正交面（±X, ±Y, ±Z）；
2. 每个位置采集足够长时间的数据以平均噪声；
3. 利用最小二乘法拟合标定参数；
4. 将校准矩阵写入嵌入式系统EEPROM供运行时调用。

此过程可通过自动化转台实现，极大提升效率与一致性。

3.3.2 温度补偿电路设计与封装工艺优化

温度变化是影响IMU长期稳定性的主要因素。零偏与比例因子均随温度非线性漂移。为此，应在PCB上集成高精度NTC或PTAT温度传感器，并构建分段线性补偿模型：

float temp_compensate(float raw, float temp) {
    int idx = (int)((temp + 40) / 10);             // 每10°C一段
    float bias_seg = bias_table[idx];              // 查表获取段内零偏
    float sf_corr = sf_table[idx];                 // 比例因子修正
    return (raw - bias_seg) * sf_corr;
}

同时，采用陶瓷封装或金属屏蔽壳体可有效减少热应力与电磁干扰，提升整体可靠性。

3.4 IMU标定流程与实验室测试验证

3.4.1 分步旋转法标定加速度计与陀螺仪参数

使用精密转台执行24位置或48位置旋转序列，采集静态与动态数据，结合Allan方差分析提取噪声成分（量化噪声、角度随机游走、零偏不稳定性等），评估IMU真实性能。

3.4.2 多位置静态与动态实验数据采集与处理

搭建恒温箱与振动台联合测试平台，模拟极端环境条件，验证补偿算法有效性。数据分析软件应具备频谱分析、协方差估计与轨迹重构功能，全面评估IMU在真实任务剖面下的表现。

4. SINS误差源分析与传感器误差补偿算法实现

惯性导航系统在缺乏外部观测信息时，其精度高度依赖于惯性测量单元（IMU）的性能。然而，现实中的加速度计和陀螺仪不可避免地存在各类误差，这些误差会随时间累积并显著影响位置、速度与姿态解算的准确性。尤其在卫星信号遮挡或不可用的场景下，如城市峡谷、隧道、地下空间等，SINS（Strapdown Inertial Navigation System）完全依靠内部惯性数据进行推算，此时原始传感器误差若未得到有效建模与补偿，将迅速导致导航漂移。因此，深入理解SINS的主要误差来源，建立合理的数学模型，并设计有效的误差补偿机制，是提升系统长期稳定性和定位精度的关键所在。

本章围绕SINS系统的误差特性展开系统性剖析，从器件级到系统级逐层拆解误差成因，构建误差传播动力学模型，进而提出可工程落地的补偿策略。通过理论建模、参数辨识与在线修正相结合的方式，揭示如何在复杂环境下维持高精度导航能力，为后续基于卡尔曼滤波的数据融合提供高质量的状态估计基础。

4.1 主要误差来源分类与建模

4.1.1 器件级误差：零偏、比例因子误差、噪声与温度漂移

惯性传感器的核心——加速度计与陀螺仪，在理想状态下应满足线性响应关系，即输出正比于输入物理量。但在实际制造工艺限制下，各类非理想因素引入了多种系统性与随机性误差。其中最为关键的包括零偏误差（Bias）、比例因子误差（Scale Factor Error）、交叉耦合误差（Cross-axis Sensitivity）、非线性失真以及由环境变化引起的温漂效应。

以MEMS型IMU为例，其典型误差模型可表示为：

y = K \cdot (a + b + n) + c

其中：
- $ y $：传感器原始输出（数字量或电压）；
- $ a $：真实加速度或角速率；
- $ K $：比例因子矩阵（通常为3×3对角阵，考虑轴间独立性）；
- $ b $：零偏向量（常值或慢变项）；
- $ n $：随机噪声成分（白噪声、角随机游走等）；
- $ c $：安装误差与交叉轴干扰项。

上述公式表明，即使在静态无运动条件下，传感器仍可能输出非零值（即零偏），该偏差具有时间相关性，部分表现为缓慢漂移（drift）。此外，比例因子不准确会导致测量值被放大或缩小，直接影响积分结果的累积误差增长速度。

为了量化这些误差的影响，常用指标如下表所示：

误差类型	描述	单位	典型值（中等精度IMU）
零偏稳定性	静态条件下零偏随时间的变化幅度	°/h 或 mg	0.1 ~ 1 °/h
比例因子误差	实际增益与标称增益之间的相对偏差	ppm 或 %	±50 ~ 200 ppm
角随机游走（ARW）	陀螺仪白噪声积分后引起的角度不确定性	°/√h	0.01 ~ 0.1 °/√h
速度随机游走（VRW）	加速度计噪声积分造成速度波动	m/s/√h	0.01 ~ 0.1 m/s/√h
温度系数	每摄氏度引起的零偏或比例因子变化率	°/h/°C 或 mg/°C	0.01 ~ 0.1 °/h/°C

这些参数不仅决定了IMU的基本性能等级，也直接决定了后续补偿算法的设计复杂度。例如，高动态应用需重点关注比例因子的非线性与时变特性；而在长时间任务中，零偏稳定性成为主导误差源。

import numpy as np

def simulate_gyro_output(true_omega, dt=0.01, bias_stability=0.1, arw=0.05, temp_drift_coeff=0.01, temperature=25):
    """
    模拟陀螺仪带误差的输出
    参数说明：
    - true_omega: 真实角速度 (rad/s)
    - dt: 采样周期 (s)
    - bias_stability: 零偏稳定性 (°/h)，转换为 rad/s
    - arw: 角随机游走 (°/√h)，表示白噪声强度
    - temp_drift_coeff: 温度漂移系数 (°/h/°C)
    - temperature: 当前工作温度 (°C)，相对于标定温度（假设25°C）
    返回：
    - sensor_output: 包含误差的陀螺仪读数
    """
    # 转换单位
    bias_stability_radps = bias_stability * (np.pi / 180) / 3600  # °/h → rad/s
    arw_rad_sqrts = arw * (np.pi / 180) / 3600**0.5  # °/√h → rad/√s
    # 随机噪声（离散白噪声）
    noise = np.random.normal(0, arw_rad_sqrts / np.sqrt(dt))
    # 温度引起的零偏漂移（简化为线性模型）
    temp_offset = temperature - 25
    temp_drift = temp_drift_coeff * temp_offset * (np.pi / 180) / 3600  # 转换为 rad/s
    # 总零偏（包含稳定项与温漂）
    total_bias = np.random.normal(0, bias_stability_radps) + temp_drift
    # 输出 = 真实值 + 偏置 + 噪声
    sensor_output = true_omega + total_bias + noise
    return sensor_output

代码逻辑逐行解析：

simulate_gyro_output 函数用于生成带有典型误差的陀螺仪模拟输出，适用于误差建模与补偿算法验证。
输入的真实角速度 true_omega 是物理量基准。
将“零偏稳定性”从 °/h 转换为 rad/s，便于与SI单位统一计算。
“角随机游走” ARW 表示陀螺白噪声功率密度，通过除以 √dt 得到当前步长下的噪声标准差。
使用 np.random.normal 生成符合高斯分布的随机噪声项，模拟电子热噪声。
引入温度偏移对零偏的影响，体现温漂效应，这是实际工程中最难校准的部分之一。
最终输出为真实角速度叠加零偏与噪声，反映了典型的MEMS陀螺行为。

该模型可用于生成仿真数据集，进一步训练或验证补偿算法的有效性。

4.1.2 系统级误差：安装误差、初始对准误差与时间同步偏差

除了器件本身的误差外，SINS作为一个集成系统，还面临来自结构装配、初始化过程及多源数据协同带来的系统级误差。

安装误差（Misalignment Errors）

理想的三轴加速度计与陀螺仪应严格正交安装，且各轴方向与载体坐标系一致。但由于机械加工与封装公差，实际存在微小角度偏差，称为轴间对准误差。这种误差会导致某一方向的真实加速度或角速度在其他轴上产生耦合输出。

设真实的物理量为 $\mathbf{a}_b$（载体系下的加速度），由于安装误差造成的感知值为：

\mathbf{\tilde{a}} = (\mathbf{I} + \mathbf{M}) \mathbf{K} (\mathbf{a}_b + \mathbf{b}_a) + \mathbf{n}_a

其中：
- $\mathbf{M}$：3×3的错位矩阵（off-alignment matrix），描述各轴间的倾斜角度；
- $\mathbf{K}$：比例因子对角阵；
- $\mathbf{b}_a$：加速度计零偏；
- $\mathbf{n}_a$：噪声项。

这类误差可通过精密标定消除，但在批量生产中难以完全避免，需在出厂前进行补偿写入固件。

初始对准误差（Initial Alignment Error）

SINS必须在启动阶段完成初始姿态确定，即将载体系与地理系（如东北天ENU）之间的旋转关系初始化。若初始姿态角存在误差（如俯仰±0.1°），则会在后续积分过程中持续引入科里奥利加速度误判，导致速度与位置快速发散。

初始对准方法分为静态对准与动态对准两类。前者利用重力矢量与地球自转角速度进行粗对准，后者结合GNSS速度信息进行精对准。任何一步的误差都会传递至导航解算全过程。

时间同步偏差（Time Synchronization Error）

当SINS与GNSS或其他辅助传感器联合工作时，数据采集的时间戳必须精确同步。若存在毫秒级延迟，将导致状态预测与观测更新不同步，严重破坏滤波器稳定性。

例如，假设IMU以100Hz运行，GNSS以10Hz输出，若未采用硬件PPS同步或软件插值对齐，则可能出现以下情况：

sequenceDiagram
    participant IMU
    participant GNSS
    participant EKF
    IMU->>EKF: 加速度/角速度 @ t=10.01s
    IMU->>EKF: ...连续更新...
    GNSS->>EKF: 位置/速度 @ t=10.00s (延迟10ms)
    EKF->>EKF: 使用旧观测更新新状态 → 不一致性！

如上流程图所示，若GNSS数据滞后于IMU积分状态，卡尔曼滤波器将基于过时的观测进行修正，可能导致协方差膨胀甚至发散。解决办法包括使用时间戳插值、延迟缓冲队列或引入异步滤波结构。

综上所述，系统级误差虽不如器件噪声直观，但其影响深远，必须在系统设计初期予以充分考虑。

4.2 误差传播特性与可观测性分析

4.2.1 误差微分方程建立与误差状态向量构建

为了有效抑制误差，必须首先明确其在导航解算过程中的传播路径。SINS的误差传播可通过构建误差状态空间模型来描述，这为后续卡尔曼滤波器的设计提供了理论依据。

定义误差状态向量如下：

\mathbf{x} = \begin{bmatrix}
\delta \mathbf{r} \
\delta \mathbf{v} \
\phi \
\delta \mathbf{b}_a \
\delta \mathbf{b}_g \
\end{bmatrix}
\in \mathbb{R}^{15}

其中：
- $\delta \mathbf{r}$：位置误差（m）；
- $\delta \mathbf{v}$：速度误差（m/s）；
- $\phi$：姿态误差角（欧拉角误差，rad）；
- $\delta \mathbf{b}_a$：加速度计零偏变化量；
- $\delta \mathbf{b}_g$：陀螺仪零偏变化量。

根据经典惯性导航误差方程（参考文献Titterton & Weston），可得线性化误差微分方程组：

\begin{aligned}
\dot{\delta \mathbf{r}} &= \delta \mathbf{v} \
\dot{\delta \mathbf{v}} &= \mathbf{f} \times \phi + \delta \mathbf{f} - (2\boldsymbol{\omega} {ie}^n + \boldsymbol{\omega} {en}^n) \times \delta \mathbf{v} + \boldsymbol{\omega} {ie}^n \times (\boldsymbol{\omega} {ie}^n \times \delta \mathbf{r}) \
\dot{\phi} &= \boldsymbol{\omega} {in}^n \times \phi - \delta \boldsymbol{\omega} {in}^n \
\dot{\delta \mathbf{b}}_a &= 0 \quad (\text{一阶马尔可夫过程更佳})\
\dot{\delta \mathbf{b}}_g &= 0
\end{aligned}

其中：
- $\mathbf{f}$：比力向量；
- $\boldsymbol{\omega}_{in}^n$：载体系至地理系的角速度投影；
- $\delta \mathbf{f}, \delta \boldsymbol{\omega}$：包含传感器误差的扰动项。

此模型揭示了姿态误差如何通过叉积影响速度更新，而速度误差又进一步导致位置漂移。特别值得注意的是，陀螺零偏误差 $\delta b_g$ 直接进入姿态更新环节，经过积分后形成角度误差 $\phi$，再间接影响速度与位置，构成“三级放大”效应。

可观测性分析表格

状态变量	是否可观测（GNSS辅助）	条件说明
位置误差	✅ 是	GNSS直接观测
速度误差	✅ 是	多普勒或差分GNSS观测
姿态误差（水平）	✅ 是	重力矢量约束+GNSS速度
姿态误差（航向）	⚠️ 仅在运动时可观测	静止时无法区分陀螺偏置与真实转向
加速度计零偏	✅ 是	重力方向长期静止可观测
陀螺仪零偏	⚠️ 运动激励下可观测	需绕各轴旋转才能激发可观测性

该表说明：并非所有状态都能被实时准确估计。例如，在车辆长时间直线行驶时，航向方向的陀螺零偏难以分离，必须通过特定机动（如转弯）提高其可观测性。因此，自主标定算法需结合运动模式识别动态调整激励策略。

4.2.2 舒勒振荡、科里奥利效应等物理现象对精度的影响

舒勒振荡（Schuler Oscillation）

当SINS在地理系下进行水平通道误差传播分析时，会出现周期约为84.4分钟的自然谐振现象，称为舒勒振荡。其根本原因是导航算法中人为引入了一个与地球曲率相匹配的振荡频率：

T_s = \frac{2\pi}{\sqrt{g/R}} \approx 84.4\,\text{min}

尽管该振荡本身不会导致系统不稳定，但它会使位置与速度误差呈现周期性波动。若未正确建模或滤波器设计不当，反而可能放大此类振荡。

科里奥利加速度误差

地球自转与载体运动共同作用产生科里奥利加速度项：

\mathbf{a} c = -2 \boldsymbol{\omega} {ie}^n \times \mathbf{v}^n

若姿态角存在微小误差 $\phi$，则该加速度在计算中会被错误投影，从而引入额外的速度增量：

\delta \dot{\mathbf{v}} \approx -2 (\boldsymbol{\omega}_{ie}^n \times \mathbf{v}^n) \times \phi

该误差随纬度与速度增大而加剧，尤其在高速飞行器或极区航行中不可忽略。

4.3 传感器误差补偿关键技术

4.3.1 分段线性化温度补偿模型设计

温度是影响IMU零偏与比例因子最主要的因素。由于MEMS材料的热膨胀系数差异，传感器输出随温度呈现非线性漂移。为此，采用分段线性化模型进行补偿是一种兼顾精度与计算效率的方法。

假设某陀螺零偏随温度变化曲线如下图所示：

graph LR
    A[低温段 -40°C] -->|线性斜率k1| B[常温段 25°C]
    B -->|斜率k2| C[高温段 85°C]
    style A fill:#f9f,stroke:#333
    style B fill:#bbf,stroke:#333
    style C fill:#f96,stroke:#333

将整个工作温度区间划分为若干段，在每一段内拟合零偏与温度的关系为：

b(T) = b_0^{(i)} + k^{(i)} (T - T_{mid}^{(i)})

其中 $i$ 表示第 $i$ 个温度段。

具体实施步骤如下：

在恒温箱中采集多个温度点下的静态零偏数据；
对每个轴分别进行分段线性回归；
将分段参数写入设备固件；
运行时读取片上温度传感器值，查找对应段并计算补偿量。

class TemperatureCompensator:
    def __init__(self):
        # 示例：X轴陀螺分段参数 [start_temp, end_temp, bias_at_mid, slope]
        self.segments = [
            [-40, 10, 0.01, 0.002],   # 单位：°/s
            [10, 50, 0.02, 0.0005],
            [50, 85, 0.03, 0.003]
        ]
    def compensate(self, raw_bias, temperature):
        for start, end, b0, k in self.segments:
            if start <= temperature < end:
                delta_t = temperature - (start + end)/2
                correction = b0 + k * delta_t
                return raw_bias - correction
        return raw_bias  # 超出范围保持原值

参数说明：
- segments ：存储每一段的温度范围与拟合参数；
- compensate() ：根据当前温度选择合适段落，减去预测漂移。

该方法相比全局多项式拟合更具鲁棒性，避免外推误差，适合嵌入式资源受限环境。

4.3.2 基于最小二乘法的标定参数辨识方法

在实验室条件下，可通过多位置静态标定获取IMU各项误差参数。以加速度计为例，将其置于多个已知方向（如±x, ±y, ±z），记录输出均值，建立超定方程组求解标定矩阵。

设理想重力大小为 $g$，第 $i$ 次姿态下的理论单位向量为 $\mathbf{u}_i$，实际输出为 $\mathbf{y}_i$，则有：

\mathbf{y}_i = \mathbf{S} \mathbf{u}_i + \mathbf{b} + \mathbf{n}_i

整理为矩阵形式：

\mathbf{Y} = \mathbf{X} \theta + \mathbf{N}

其中：
- $\mathbf{Y}$：所有输出向量堆叠；
- $\mathbf{X}$：设计矩阵，每行为 $[\mathbf{u} i^T, 1]$；
- $\theta = [\mathbf{s} {xx}, \mathbf{s}_{xy}, …, b_x, b_y, b_z]^T$：待估参数；
- $\mathbf{S}$：3×3灵敏度矩阵（含比例因子与交叉轴）。

最小二乘解为：

\hat{\theta} = (\mathbf{X}^T \mathbf{X})^{-1} \mathbf{X}^T \mathbf{Y}

def calibrate_accelerometer(positions, measurements):
    """
    positions: list of unit vectors [gx, gy, gz] in gravity direction
    measurements: list of raw sensor outputs [ax, ay, az]
    """
    X = []
    Y = []
    for u, y in zip(positions, measurements):
        row = [u[0], u[1], u[2], 1]  # [ux, uy, uz, 1] for bias
        X.append(row + [0]*3 + [0]*3)  # expand to full sensitivity matrix (simplified)
        Y.extend(y)
    X = np.array(X)
    Y = np.array(Y)
    theta = np.linalg.inv(X.T @ X) @ X.T @ Y
    return theta

该方法可扩展至陀螺速率台标定，实现全参数自动化辨识。

4.4 实际工程中误差抑制策略

4.4.1 自主标定算法在车载环境下的适应性改进

传统标定需精密转台，成本高昂。针对车载应用场景，提出一种基于驻车检测的自主标定方法：

利用GNSS速度<0.1m/s判断车辆静止；
收集连续10秒IMU数据，计算零偏均值；
若连续三天同一地点停靠，统计温漂趋势；
动态更新补偿参数表。

优势在于无需人工干预，持续优化长期稳定性。

4.4.2 在线估计与反馈修正机制实现

结合扩展卡尔曼滤波器（EKF），将零偏作为状态变量进行在线估计，并反馈至导航解算环路：

# 伪代码示意
ekf.predict()
if gnss_available:
    residual = gnss_velocity - ins_velocity
    ekf.update(residual)
    # 更新后的陀螺零偏反馈回IMU预处理
    imu.bias_gyro = ekf.state[12:15]

实现闭环控制，显著延长纯惯性导航可用时间。

以上内容全面覆盖了SINS误差源建模、传播分析与补偿技术，形成了从理论到实践的完整链条，为高精度组合导航系统奠定了坚实基础。

5. 基于卡尔曼滤波的多源数据融合与SINS综合应用实践

5.1 卡尔曼滤波理论基础及其在SINS中的适用性

卡尔曼滤波（Kalman Filter, KF）作为一种最优线性递归估计器，广泛应用于动态系统的状态估计中。其核心思想是在系统噪声和观测噪声均服从高斯分布的前提下，通过最小化估计误差协方差矩阵来实现对系统状态的最优估计。在卫星惯性导航系统（SINS）中，由于IMU输出具有高频但累积误差显著的特点，而GNSS提供低频但绝对位置/速度信息，因此采用卡尔曼滤波进行数据融合成为提升整体导航精度的关键手段。

5.1.1 线性卡尔曼滤波与扩展卡尔曼滤波（EKF）原理对比

标准线性卡尔曼滤波适用于状态转移和观测模型均为线性的情况，其基本流程包括预测与更新两个阶段：

预测步 ：
$$
\hat{x} {k|k-1} = A_k \hat{x} {k-1|k-1}, \quad P_{k|k-1} = A_k P_{k-1|k-1} A_k^T + Q_k
$$
更新步 ：
$$
K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1}
$$
$$
\hat{x} {k|k} = \hat{x} {k|k-1} + K_k (z_k - H_k \hat{x} {k|k-1}), \quad P {k|k} = (I - K_k H_k) P_{k|k-1}
$$

其中，$\hat{x}$为状态估计值，$P$为误差协方差矩阵，$A$为状态转移矩阵，$H$为观测矩阵，$Q$和$R$分别为过程噪声与观测噪声协方差。

然而，在SINS中，姿态通常以四元数或欧拉角表示，导致系统方程非线性，必须使用扩展卡尔曼滤波（Extended Kalman Filter, EKF）。EKF通过对非线性函数进行一阶泰勒展开，在当前估计点附近进行局部线性化处理。

例如，姿态更新采用四元数微分方程：
\dot{q} = \frac{1}{2} \Omega(\omega) q
该方程是非线性的，需在线性化后用于EKF预测阶段的状态传播。

下表对比了KF与EKF在SINS中的主要特性：

特性	线性卡尔曼滤波（KF）	扩展卡尔曼滤波（EKF）
模型要求	线性系统	非线性系统可局部线性化
计算复杂度	低	中等（需雅可比矩阵计算）
收敛性	最优（高斯线性）	局部最优，依赖初值
应用场景	GNSS位置滤波	SINS/GNSS紧耦合融合
实现难度	简单	较复杂，需Jacobian推导
数值稳定性	高	受线性化误差影响
噪声假设	高斯白噪声	近似高斯
更新频率支持	高频	高频
对初值敏感度	低	较高
工程成熟度	极高	高
典型误差来源	模型失配	截断误差、发散风险
适用传感器组合	GNSS-only	IMU+GNSS+其他

5.1.2 状态空间模型构建：系统方程与观测方程设计

在GNSS/SINS组合系统中，常用的状态向量包含位置误差、速度误差、姿态误差、陀螺零偏、加速度计零偏等：

\delta x = [\delta r_E, \delta r_N, \delta r_U, \delta v_E, \delta v_N, \delta v_U, \phi_E, \phi_N, \phi_U, b_a, b_g]^T

对应的连续时间系统方程可建模为：

\dot{\delta x}(t) = F(t)\delta x(t) + G(t)w(t)

其中，$F(t)$为系统动力学矩阵，包含舒勒振荡项、地球自转影响、科里奥利加速度等物理效应；$w(t)$为过程噪声，代表传感器随机误差（如角随机游走、速度随机游走）。

离散化后用于EKF预测：

# Python伪代码示例：EKF预测步骤
import numpy as np

def ekf_predict(x, P, F, G, Q, dt):
    # 状态转移矩阵近似
    Phi = np.eye(F.shape[0]) + F * dt
    x_pred = Phi @ x
    P_pred = Phi @ P @ Phi.T + G @ Q @ G.T * dt
    return x_pred, P_pred

观测方程根据耦合模式不同而变化。在松耦合模式下，观测量为GNSS与SINS的位置/速度之差：

z = [r_{GNSS} - r_{SINS}, v_{GNSS} - v_{SINS}]^T

对应观测矩阵 $H$ 从状态向量中提取相关误差项。

而在紧耦合中，观测量为伪距与多普勒残差，涉及卫星几何关系，$H$矩阵维度更高且随时间变化。

mermaid格式流程图展示EKF在SINS/GNSS融合中的执行逻辑：

graph TD
    A[IMU原始数据] --> B[捷联解算]
    C[GNSS定位结果] --> D{耦合模式判断}
    D -->|松耦合| E[位置/速度差作为观测量]
    D -->|紧耦合| F[伪距/多普勒残差作为观测量]
    B --> G[EKF预测步: 状态传播]
    G --> H[EKF更新步: 卡尔曼增益计算]
    H --> I[状态校正: 修正姿态、速度、位置]
    I --> J[输出融合导航结果]
    J --> K[反馈至IMU解算模块]

该闭环结构确保了误差被持续估计并补偿，从而抑制SINS的漂移趋势。尤其在GNSS信号短暂丢失时，系统仍能依靠IMU积分维持较高精度导航达数十秒甚至更久，体现出强大的鲁棒性。

本文还有配套的精品资源，点击获取