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

简介：卡尔曼滤波是一种高效的递归滤波算法，广泛应用于动态系统状态估计中。在捷联惯导（SINS）与GPS的组合导航系统中，通过融合SINS连续高频率输出与GPS高精度定位的优势，卡尔曼滤波有效抑制传感器噪声与误差累积，提升导航系统的精度与鲁棒性。本文介绍基于MATLAB平台实现的SINS/GPS组合导航系统中卡尔曼滤波的设计与应用，涵盖状态建模、测量更新、预测校正流程及扩展卡尔曼滤波（EKF）处理非线性问题的方法，并结合“killman”压缩包中的代码实例，帮助理解其在实际工程中的部署过程。

1. 卡尔曼滤波基本原理与数学模型

卡尔曼滤波的基本思想与最优估计准则

卡尔曼滤波（Kalman Filter, KF）是一种基于状态空间模型的递归贝叶斯估计算法，适用于线性动态系统在高斯白噪声环境下的最优状态估计。其核心在于融合系统动力学模型预测与含噪观测信息，通过最小化估计误差的协方差矩阵，实现对系统真实状态的最优线性无偏估计（BLUE）。KF假设系统过程噪声 $ w_k \sim \mathcal{N}(0, Q) $ 和观测噪声 $ v_k \sim \mathcal{N}(0, R) $ 均为零均值高斯白噪声，且相互独立，从而保证后验状态分布始终为高斯分布，仅需维护均值（状态估计）和协方差（不确定性度量）即可完成全部推断。

% 简化的卡尔曼滤波预测与更新步骤示例
x_pred = F * x_prev;           % 状态预测
P_pred = F * P_prev * F' + Q;  % 协方差预测
K = P_pred * H' / (H * P_pred * H' + R);  % 计算卡尔曼增益
x_upd = x_pred + K * (z - H * x_pred);    % 状态更新
P_upd = (eye(n) - K * H) * P_pred;        % 协方差更新

上述代码体现了KF五大公式的数值实现逻辑：状态预测、协方差预测、卡尔曼增益计算、状态更新与协方差更新。其中， 卡尔曼增益 $ K $ 自动权衡预测与观测的可信度——当观测噪声小（R小），增益变大，更信任测量；反之则依赖模型预测，展现出自适应加权特性。

滤波器的递归结构与计算优势

不同于批处理方法（如最小二乘），KF采用“预测-更新”循环结构，每一步仅依赖前一时刻的状态与当前观测，避免存储历史数据，显著降低内存开销与计算复杂度，适合实时嵌入式系统部署。

线性高斯假设及其局限性

KF的最优性严格依赖于系统的 线性性 与噪声的 高斯性 。对于非线性系统（如SINS/GPS组合导航中的姿态更新），必须引入扩展卡尔曼滤波（EKF）或无迹卡尔曼滤波（UKF）等改进算法，通过对非线性函数局部线性化（雅可比矩阵）或统计线性化来近似处理。本章所建立的理论框架为后续非线性滤波方法的设计提供数学基础与实现参照。

2. SINS/GPS组合导航系统架构与数据特点

在现代高精度导航技术中，单一传感器已难以满足复杂动态环境下的全天候、全时段、高可靠性定位需求。捷联惯性导航系统（Strapdown Inertial Navigation System, SINS）与全球定位系统（Global Positioning System, GPS）的融合构成了当前主流的组合导航解决方案。本章将系统阐述SINS和GPS各自的工作机制、误差特性及其时空互补优势，并深入剖析两者融合所依赖的系统架构设计原则。通过分析不同组合模式的技术差异与适用边界，揭示多源信息融合如何实现优于单系统的综合性能提升。

2.1 捷联惯导系统（SINS）的工作原理与误差源分析

捷联惯导系统是一种无需机械稳定平台的自主式导航系统，其核心由三轴陀螺仪和三轴加速度计构成，直接固连于载体上，通过数学平台完成姿态解算。SINS利用惯性测量单元（IMU）实时采集角速度和比力信号，在初始对准后递推计算出载体的姿态、速度与位置信息。由于其完全依赖内部传感器积分运算，具有不依赖外部信号、抗干扰能力强的优点，但也因此存在误差随时间累积的本质缺陷。

2.1.1 SINS的姿态、速度与位置解算流程

SINS的核心是基于牛顿力学框架进行运动学递推。整个解算过程可分为三个层次：姿态更新、速度更新与位置更新，三者以特定频率同步执行，形成所谓的“机械编排”算法。

姿态解算是最基础也是最关键的步骤。它通过陀螺输出的角速率 $\omega_{ib}^b$（载体系 $b$ 相对于惯性系 $i$ 在载体系下的投影），结合方向余弦矩阵 $C_b^n$ 或四元数 $q_{nb}$，递推求解姿态变换关系：

\dot{q} {nb} = \frac{1}{2} q {nb} \otimes \begin{bmatrix} 0 \ \omega_{ib}^b \end{bmatrix}

该微分方程通常采用四阶龙格-库塔法或欧拉法离散化求解，确保姿态更新的数值稳定性。

速度更新则在地理系（n系）中进行，依据牛顿第二定律：

\dot{v}^n = C_b^n f^b - (2\omega_{ie}^n + \omega_{en}^n) \times v^n + g^n

其中 $f^b$ 为加速度计测得的比力，$C_b^n$ 为姿态矩阵，$\omega_{ie}^n$ 为地球自转角速度在n系的投影，$\omega_{en}^n$ 为导航系相对于地球的旋转角速度，$g^n$ 为当地重力矢量。此公式考虑了科里奥利力与离心加速度的影响。

位置更新通过对速度积分实现：

\begin{aligned}
\dot{\phi} &= \frac{v_N}{R_M + h} \
\dot{\lambda} &= \frac{v_E}{(R_N + h)\cos\phi} \
\dot{h} &= v_U
\end{aligned}

其中 $\phi, \lambda, h$ 分别表示纬度、经度和高度；$v_N, v_E, v_U$ 为北向、东向、天向速度分量；$R_M, R_N$ 为子午圈与卯酉圈曲率半径。

上述流程形成了一个闭环递推结构，如以下mermaid流程图所示：

graph TD
    A[IMU原始数据] --> B[陀螺角速率 ω]
    A --> C[加速度计比力 f]
    B --> D[姿态更新: 四元数/DCM]
    C --> E[速度更新: n系速度]
    D --> E
    E --> F[位置更新: 纬度/经度/高度]
    F --> G[输出导航结果]
    G --> H[反馈至姿态矩阵 C_b^n]
    H --> D

该流程体现了SINS的高度自洽性，但同时也暴露了其致命弱点：所有状态均基于积分获得，任何微小的零偏或噪声都会随时间不断放大。

2.1.2 惯性器件误差建模：零偏、标度因数、随机游走

惯性器件的测量误差直接影响SINS的长期精度，主要可归纳为系统性误差与随机性误差两大类。

误差类型	物理含义	数学模型	影响程度
零偏（Bias）	静态时输出非零值	常值项或一阶马尔可夫过程	高（导致速度与位置漂移）
标度因数误差（Scale Factor Error）	输入与输出比例失真	$ y = (1+k)x + b $	中高（随输入幅值增大而加剧）
安装误差（Misalignment）	传感器轴间不正交	轴间耦合矩阵	中（影响姿态解耦）
随机游走（Random Walk）	白噪声积分形成	$ dx/dt = w(t), w\sim N(0,q) $	长期主导因素
量化噪声	AD转换引入	均匀分布小幅度波动	低频下可忽略

以陀螺仪为例，其实际输出可建模为：

\tilde{\omega} = (I + K_g) \cdot \omega + b_g + n_g

其中：
- $\tilde{\omega}$：测量值；
- $K_g$：标度因数误差矩阵；
- $b_g$：零偏项，常假设为一阶马尔可夫过程：$\dot{b} g = -\frac{1}{\tau_g} b_g + w {bg}$；
- $n_g$：白噪声项，服从 $N(0, q_g)$。

类似地，加速度计误差模型为：

\tilde{f} = (I + K_a) \cdot f + b_a + n_a

这些误差在导航解算中会逐步传播并积累。例如，陀螺零偏 $b_g$ 会导致姿态误差 $\theta = b_g \cdot t$，进而引起速度误差 $v_{err} = \theta \cdot g \cdot t$，最终造成位置误差 $p_{err} \propto g \cdot b_g \cdot t^2$ ——呈平方增长趋势。

为了抑制此类误差，必须借助外部参考源（如GPS）对其进行在线估计与补偿，这正是组合导航的核心动机之一。

2.1.3 机械编排中的累积误差机制

“机械编排”（Mechanization）是指SINS内部用于递推导航参数的一整套数学算法流程。尽管没有物理平台，但其逻辑结构仍模拟了传统平台惯导的功能模块。然而，正是这种连续积分机制导致了误差的指数级增长。

考虑一个简单的水平通道误差传播模型。假设北向陀螺存在恒定零偏 $b_x$，则姿态误差为：

\delta\theta_x = b_x \cdot t

该姿态误差会错误地将部分重力分量 $g$ 投影到东向加速度计上，产生虚假的横向加速度感知：

a_y^{false} = g \cdot \delta\theta_x = g b_x t

积分后得到东向速度误差：

\delta v_y = \int_0^t a_y^{false} dt = \frac{1}{2} g b_x t^2

再次积分得位置误差：

\delta p_y = \frac{1}{6} g b_x t^3

可见，仅一个方向的陀螺零偏即可引发立方级的位置漂移。这一现象被称为“舒勒振荡激励下的发散误差”，在无外部校正的情况下无法收敛。

下表展示了典型MEMS级IMU在自由惯性导航下的误差增长情况（假设初始对准准确）：

时间	位置误差（估算）	主要来源
1秒	<1米	量化噪声
10秒	~5米	零偏启动
1分钟	~50米	角度误差积累
10分钟	>1公里	重力误调制
1小时	数十公里	完全失控

由此可见，SINS虽具备毫秒级响应速度和高频输出能力（可达100Hz以上），但其独立工作能力极为有限，必须与GPS等绝对定位系统结合使用。

2.2 GPS导航系统的定位机制与时空特性

全球定位系统（GPS）作为典型的卫星无线电导航系统，通过接收来自多颗卫星的无线电信号，测定信号传播时间来估算距离，从而实现三维空间定位。与SINS相反，GPS提供的是绝对位置和速度信息，且长期稳定性极佳，但其更新率较低、易受遮挡影响，且存在瞬时跳变风险。

2.2.1 GPS伪距与载波相位测量原理

GPS接收机最基本的观测量包括伪距（Pseudorange）和伪距率（Pseudorange Rate），以及更精密的载波相位（Carrier Phase）。

伪距定义为：

\rho_i = c \cdot (t_r - t_t) + I + T + \epsilon

其中：
- $c$：光速；
- $t_r, t_t$：接收机与卫星端的信号收发时刻；
- $I$：电离层延迟；
- $T$：对流层延迟；
- $\epsilon$：多路径、噪声等残余误差。

理想情况下，$\rho_i$ 应等于卫星到接收机的真实几何距离 $r_i$ 加上接收机钟差 $c \cdot \delta t_r$。但由于卫星钟差、大气延迟等因素，称之为“伪”距。

伪距观测方程为：

\rho_i = | \vec{r}_i - \vec{r} | + c \cdot (\delta t_r - \delta t^s) + I_i + T_i + \epsilon_i

其中 $\vec{r}_i$ 为第$i$颗卫星位置，$\vec{r}$ 为接收机位置，$\delta t^s$ 为卫星钟差（通常由导航电文校正）。

若同时观测至少4颗卫星，则可通过最小二乘法求解四维变量：$(x, y, z, \delta t_r)$。

相比之下，载波相位测量精度更高（毫米级），其观测值为：

\phi_i = \frac{1}{\lambda} \left( | \vec{r}_i - \vec{r} | + c \cdot (\delta t_r - \delta t^s) + I_i + T_i \right) + N_i + \eta_i

其中 $N_i$ 为整周模糊度（未知整数），$\eta_i$ 为测量噪声。虽然精度极高，但需解决模糊度固定问题，适用于RTK等高精度场景。

2.2.2 定位精度影响因素：多路径效应、电离层延迟、卫星几何分布（DOP）

GPS定位精度不仅取决于接收机本身，还受到多种外部环境因素制约。

多路径效应 ：当卫星信号经建筑物、地面反射后再被接收机捕获，会产生额外路径差，导致伪距测量偏差。城市峡谷环境中尤为严重，误差可达数米。

电离层延迟 ：电离层中的自由电子使电磁波传播速度变慢，造成伪距偏大。其大小与频率平方成反比，白天太阳活动强烈时可达数十米。双频接收机可通过线性组合消除大部分电离层误差。

对流层延迟 ：由大气湿度、温度、压力引起，集中在地表10km以内，可通过模型（如Hopfield、Saastamoinen）修正。

卫星几何分布（DOP） ： Dilution of Precision 是衡量卫星空间构型优劣的关键指标。常见的有PDOP（位置）、HDOP（水平）、VDOP（垂直）。DOP越小，几何构型越好，定位精度越高。

下表列出了不同DOP值对应的精度衰减因子：

DOP范围	定位质量	典型误差放大倍数
<2	优秀	1.0–1.5
2–3	良好	1.5–2.0
3–5	一般	2.0–3.0
5–9	较差	3.0–5.0
>9	不可用	>5.0

例如，若伪距测量标准差为3m，PDOP=6，则位置误差标准差约为 $3 \times 6 = 18m$。

2.2.3 数据输出频率与信号可用性分析

GPS接收机的数据输出频率通常为1Hz~20Hz，远低于SINS的100Hz甚至更高采样率。这意味着在高速机动或强振动环境下，GPS无法及时反映动态变化。

此外，信号可用性受环境限制显著：
- 遮挡问题 ：隧道、地下车库、高楼密集区会导致信号丢失；
- 动态中断 ：快速转弯或加减速可能引起信号失锁；
- 冷启动延迟 ：首次定位需下载星历，耗时可达30秒以上。

因此，尽管GPS能有效抑制SINS的长期漂移，但在信号中断期间，系统将退化为纯惯性导航，误差迅速扩大。这就要求组合系统具备良好的预测与平滑能力。

2.3 SINS与GPS的互补性与融合必要性

SINS与GPS在时间响应、误差特性和信息类型方面呈现出强烈的互补关系，使得两者的融合成为高性能导航系统的必然选择。

2.3.1 SINS短时高精度 vs 长期漂移特性

SINS的优势在于其 高动态响应能力 和 连续输出特性 。在短时间内（几秒内），其姿态、速度解算精度极高，尤其适合飞行器、无人机等需要高频反馈的应用场景。此外，SINS完全自主运行，不受电磁干扰或人为遮蔽影响。

然而，由于其基于积分的机制，所有误差都会随时间累积，表现为 无界增长 。即使高端光纤陀螺（FOG）或激光陀螺（RLG）也难以避免缓慢漂移。因此，SINS不适合作为长期独立导航手段。

2.3.2 GPS长期稳定 vs 动态响应慢与信号中断风险

GPS的优势在于提供 全局一致的绝对坐标 ，且误差不会随时间增长。只要信号持续可用，其位置精度可长期保持在亚米至厘米级（视接收机类型而定）。

但其劣势同样明显：
- 更新频率低，难以捕捉高频动态；
- 易受环境干扰，存在周期性或突发性中断；
- 冷启动时间长，不适合频繁开关机应用；
- 存在周跳、粗差等异常数据，需专门检测与修复。

2.3.3 多源信息融合的性能增益预期

通过卡尔曼滤波等最优估计算法，可以将SINS与GPS的信息有机融合，达到“1+1>2”的效果。

具体性能增益体现在以下几个方面：

性能维度	SINS单独	GPS单独	组合系统
更新频率	高（≥100Hz）	低（1–20Hz）	高（继承SINS）
长期精度	差（漂移）	好（稳定）	好（被校正）
抗干扰性	强（自主）	弱（依赖信号）	强（中断时靠SINS）
动态响应	快	慢	快
可靠性	中（累积误差）	中（易跳变）	高（相互验证）

更重要的是，组合系统可通过滤波器实时估计SINS的误差状态（如姿态误差、速度误差、零偏等），并将其反馈回SINS进行校正，形成闭环控制，显著延长可用时间。

例如，在GPS信号丢失5分钟后，纯SINS位置误差可能超过500米，而经过良好校准的组合系统误差可控制在50米以内。

2.4 组合导航系统的典型架构模式

根据SINS与GPS之间信息交互的深度与方式，组合导航系统可分为松组合、紧组合与深组合三种典型架构。

2.4.1 松组合、紧组合与深组合的区别与适用场景

架构类型	信息融合层级	观测量	计算复杂度	抗干扰能力	适用场景
松组合（Loose Coupling）	位置/速度级	$\Delta P = P_{GPS} - P_{SINS}$	低	一般	开阔区域车载导航
紧组合（Tight Coupling）	伪距/伪距率级	$\Delta \rho = \rho_{meas} - \rho_{pred}$	中	强	城市峡谷、部分遮挡
深组合（Deep Coupling）	中频信号级	I/Q通道相关值	高	极强	高动态、强干扰军事应用

松组合

最为简单常见。SINS和GPS各自独立解算位置与速度，滤波器以两者之差作为观测量：

z_k = \begin{bmatrix} P_{GPS} \ V_{GPS} \end{bmatrix} - \begin{bmatrix} P_{SINS} \ V_{SINS} \end{bmatrix}

优点是模块化强、易于实现；缺点是在GPS信号弱时无法有效辅助SINS。

紧组合

直接使用每颗卫星的伪距和伪距率作为观测量。SINS预测各卫星到接收机的几何距离，与实测伪距比较：

z_i = \rho_i^{meas} - \left( | \vec{r} i - \vec{r} {SINS} | + c \cdot \delta t_r \right)

此时即使只有3颗卫星也可定位（借助SINS提供的先验位置）。抗遮挡能力更强，适用于城市环境。

深组合

将GPS接收机的跟踪环路与惯导深度融合，利用SINS辅助码环和载波环的预测，降低跟踪门限，增强弱信号捕获能力。属于射频级融合，硬件耦合度高，主要用于军用抗干扰系统。

下图为三种架构的信息流对比：

graph LR
    subgraph 松组合
        SINS1[SINS] -->|P,V| LCFilter[卡尔曼滤波器]
        GPS1[GPS] -->|P,V| LCFilter
        LCFilter -->|反馈校正| SINS1
    end

    subgraph 紧组合
        SINS2[SINS] --> TCFilter[卡尔曼滤波器]
        RawGPS[原始伪距] --> TCFilter
        TCFilter -->|反馈校正| SINS2
    end

    subgraph 深组合
        IMU[IMU] --> DeepFilter[深组合滤波器]
        RF[射频前端] --> DeepFilter
        DeepFilter -->|辅助跟踪环| RF
        DeepFilter -->|姿态/位置| Output
    end

2.4.2 反馈校正结构的设计逻辑与闭环控制优势

无论何种组合模式，反馈校正是提升性能的关键环节。典型的反馈结构如下：

% 示例：松组合反馈校正代码片段
function [corrected_att, corrected_vel, corrected_pos] = feedback_correction(x_est, raw_att, raw_vel, raw_pos)
    % x_est: 卡尔曼滤波输出的状态误差估计 [δP; δV; δθ; bg; ba]
    delta_att = x_est(7:9);  % 姿态误差（弧度）
    delta_vel = x_est(4:6);  % 速度误差（m/s）
    delta_pos = x_est(1:3);  % 位置误差（m）
    % 修正姿态（小角度近似）
    corrected_att = raw_att - rad2deg(delta_att);
    % 修正速度与位置
    corrected_vel = raw_vel - delta_vel;
    corrected_pos = raw_pos - delta_pos;
end

逻辑分析 ：
- x_est 为滤波器估计的状态误差向量；
- 使用负号进行“减去误差”操作，实现校正；
- 姿态误差从小角度近似下直接叠加；
- 若使用四元数，则应采用误差四元数左乘方式进行更新。

参数说明 ：
- raw_att , raw_vel , raw_pos ：SINS原始解算结果；
- 输出为经过校正后的平滑导航参数；
- 此过程实现了从滤波器到SINS的闭环反馈，抑制了误差积累。

闭环控制的优势在于：
- 实时修正SINS误差源（尤其是零偏）；
- 提高整体系统的鲁棒性；
- 在GPS中断期间，SINS已处于较优状态，延缓误差发散；
- 支持事后精密处理与轨迹重构。

综上所述，SINS/GPS组合导航系统通过合理架构设计与信息融合策略，充分发挥了两类传感器的互补优势，成为现代高精度导航的核心技术路线。后续章节将进一步展开状态建模与滤波实现细节。

3. 状态向量定义与系统状态方程构建

在SINS/GPS组合导航系统中，卡尔曼滤波的核心任务是通过对系统状态的最优估计，补偿惯性导航系统的累积误差，并融合GPS提供的长期稳定的绝对位置与速度信息。实现这一目标的前提是建立一个准确、完整且物理意义清晰的状态空间模型。该模型不仅决定了滤波器能否有效捕捉系统动态特性，还直接影响其收敛性、稳定性和最终导航精度。因此，合理定义状态变量、构建连续时间下的系统动力学方程，并将其精确离散化为适用于数字实现的形式，是整个滤波设计过程中最关键的环节之一。

本章将从状态变量的选择原则出发，深入探讨姿态、速度、位置误差以及传感器偏差等关键因素在状态向量中的建模方式；随后推导基于Phi角误差模型的连续时间系统动态方程，涵盖姿态误差传播、速度与位置误差演化机制，以及陀螺仪和加速度计零偏的时间行为；接着介绍如何通过一阶泰勒展开或矩阵指数法将连续模型转换为离散形式，并分析采样周期对模型保真度的影响；最后讨论过程噪声协方差矩阵 $ Q $ 的构造策略，包括各类误差源功率谱密度的量化方法及其在实际工程调参中的敏感性分析。

3.1 状态变量的选择原则与维度确定

状态变量的选取直接决定了卡尔曼滤波器能否全面描述系统的不确定性来源。在SINS/GPS组合导航中，状态向量通常由三类误差构成： 平台误差（姿态、速度、位置） 、 惯性传感器误差（陀螺零偏、加速度计零偏、标度因数误差） 和 其他辅助误差项（如GPS接收机钟差） 。这些误差共同构成了滤波器需要估计的对象。

3.1.1 位置、速度、姿态误差作为核心状态

在惯性导航系统中，姿态、速度和位置是基本输出量，但由于积分运算的存在，微小的初始误差或传感器偏差会随时间不断累积，导致长时间运行后出现显著漂移。为了纠正这种漂移，必须将它们的 误差量 作为状态进行在线估计与反馈校正。

• 姿态误差 ：通常用小角度误差表示，即所谓的“Phi角”（$\phi$），单位为弧度。它描述了真实姿态四元数与计算姿态之间的偏差方向。
• 速度误差 ：指SINS解算出的速度与参考（如GPS）速度之间的差异，单位为 m/s。
• 位置误差 ：表现为地理坐标系下的纬度、经度和高度误差，也可转换为东北天（ENU）局部切平面坐标系中的位移误差，单位为米。

这三者构成了状态向量中最基础的部分。例如，在典型15维误差状态模型中，前9个状态即为：
\delta \mathbf{r} = [\delta L, \delta \lambda, \delta h]^T, \quad
\delta \mathbf{v} = [v_N, v_E, v_D]^T, \quad
\boldsymbol{\phi} = [\phi_N, \phi_E, \phi_D]^T
其中 $L$ 为纬度，$\lambda$ 为经度，$h$ 为高度，下标 N、E、D 分别代表北、东、地三个方向。

逻辑说明 ：选择误差而非原始值作为状态，是因为卡尔曼滤波本质上是对“不确定性的修正”。我们并不直接估计真实的位置，而是估计当前SINS解算结果偏离真实值的程度，然后通过反馈回路对其进行补偿。

% 示例：初始化状态向量（15维）
n_states = 15;
x_hat = zeros(n_states, 1); % 初始状态估计
x_hat(1:3)  = [0.01; 0.01; 0.02];   % 位置误差（m）
x_hat(4:6)  = [0.1; 0.1; 0.15];     % 速度误差（m/s）
x_hat(7:9)  = deg2rad([0.5; 0.5; 1]); % 姿态误差（rad）
x_hat(10:12)= [0.01; 0.01; 0.01];    % 陀螺零偏（°/h -> rad/s）
x_hat(13:15)= [0.001; 0.001; 0.001]; % 加速度计零偏（m/s²）

代码解释 ：
- zeros(n_states, 1) 初始化一个15×1的零向量；
- 使用典型初始误差设定：位置误差约几厘米到分米级，速度误差小于0.2 m/s，姿态误差小于1度；
- 陀螺零偏常以 °/h 表示，需转换为 rad/s 参与计算（ deg2rad(x)/3600 ）；
- 所有状态均为小量，符合线性化假设前提。

3.1.2 惯性器件误差建模：零偏与刻度系数误差的必要性

惯性测量单元（IMU）中的陀螺仪和加速度计存在多种系统误差，其中对导航精度影响最大的是 零偏稳定性 和 标度因数误差 。

误差类型	物理含义	时间特性	是否建模
零偏（Bias）	静止状态下输出非零值	缓慢变化（随机游走）	必须建模
标度因数误差	输出与输入比例关系不准确	相对稳定	视需求建模
随机游走	白噪声积分形成，长期漂移主要来源	连续随机过程	包含在Q中
安装误差	轴间非正交	校准阶段处理	可预补偿

在高精度应用中（如航空、航天、无人车精密定位），必须将陀螺和加速度计的零偏作为状态变量进行在线估计。其数学模型一般采用 一阶马尔可夫过程 或 随机游走模型 ：

\dot{\mathbf{b}} g = -\frac{1}{\tau_g}\mathbf{b}_g + \mathbf{w} {bg}, \quad
\dot{\mathbf{b}} a = -\frac{1}{\tau_a}\mathbf{b}_a + \mathbf{w} {ba}

其中 $\tau$ 为相关时间常数，$\mathbf{w}$ 为驱动白噪声。若忽略衰减项（即设 $\tau \to \infty$），则退化为随机游走模型：
\dot{\mathbf{b}} g = \mathbf{w} {bg}, \quad \dot{\mathbf{b}} a = \mathbf{w} {ba}

参数说明 ：
- 若使用一阶马尔可夫模型，需知道 $\tau$ 和驱动噪声强度 $q_{bg}$；
- 实际工程中，低成本IMU多采用随机游走简化模型；
- 标度因数误差若变化缓慢，可视为常数偏差，在标定阶段消除；否则也应加入状态向量。

% 定义传感器误差模型参数
tau_gyro = 1000;      % 陀螺相关时间（秒）
Q_bg = (1e-6)^2;      % 陀螺驱动噪声方差 (rad²/s³)
tau_accel = 500;      % 加速度计相关时间
Q_ba = (1e-4)^2;      % 加速度计驱动噪声方差 (m²/s⁵)

% 构造对应的连续时间状态转移子矩阵
A_bias_gyro = -eye(3)/tau_gyro;
A_bias_accel= -eye(3)/tau_accel;

逻辑分析 ：
- 上述代码定义了一阶马尔可夫过程的系统矩阵；
- 对角结构表明各轴独立演化；
- 在后续构建总状态方程时，这些子块将嵌入整体雅可比矩阵 $ \mathbf{F} $ 中。

3.1.3 系统状态维数对滤波稳定性的影响

状态维数的选择是一把双刃剑：维数过低无法充分描述系统不确定性，导致残差过大甚至发散；维数过高则增加计算负担，可能引发数值不稳定或可观测性不足的问题。

常见的状态维度配置如下：

组合模式	状态维度	主要包含内容
简化模型	9维	仅姿态、速度、位置误差
中等精度	12维	+ 陀螺零偏
高精度	15维	+ 加速度计零偏
超高精度	18~24维	+ 标度因数、安装误差、GPS钟漂等

可观测性分析流程图（Mermaid）

graph TD
    A[选择状态变量] --> B{是否可观测？}
    B -- 是 --> C[纳入状态向量]
    B -- 否 --> D[考虑外部激励或重构观测模型]
    D --> E[增加GPS更新频率或引入零速修正]
    E --> F[重新评估可观测性]
    F --> B

流程图说明 ：
- 可观测性是指系统能否从观测数据中唯一辨识出某一状态；
- 例如，在静止或匀速运动场景下，某些姿态误差方向可能不可观；
- 解决方案包括提高运动激励、引入零速检测（ZUPT）、增强GNSS可用性等。

此外，随着维度上升，协方差矩阵 $ P $ 的规模呈平方增长（如15维 → $ 15 \times 15 = 225 $ 元素），对内存和计算效率提出更高要求。因此，应在满足精度需求的前提下尽可能精简状态空间。

3.2 连续时间域下的系统动力学模型推导

在完成状态变量选择后，下一步是建立系统的连续时间动态方程，形式为：
\dot{\mathbf{x}}(t) = \mathbf{F}(t)\mathbf{x}(t) + \mathbf{G}(t)\mathbf{w}(t)
其中 $\mathbf{x}$ 为状态向量，$\mathbf{F}$ 为系统状态转移矩阵（雅可比矩阵），$\mathbf{G}$ 为噪声分布矩阵，$\mathbf{w}$ 为过程噪声向量。

3.2.1 姿态误差角方程（Phi角模型）的建立

在捷联惯导中，姿态通常用四元数或方向余弦矩阵表示。然而，在误差分析中更常用的是 小角度误差模型 ，即 Phi 角模型（$\boldsymbol{\phi}$）。其微分方程为：

\dot{\boldsymbol{\phi}} = -\mathbf{\Omega} {ib}^b \times \boldsymbol{\phi} + \delta \boldsymbol{\omega} {ib}^b - \mathbf{C}_b^n \delta \mathbf{b}_g

其中：
- $\mathbf{\Omega} {ib}^b$：载体相对于惯性系的角速度（由陀螺测量）；
- $\delta \boldsymbol{\omega} {ib}^b$：角速度测量误差；
- $\mathbf{C}_b^n$：体坐标系到导航系的变换矩阵；
- $\delta \mathbf{b}_g$：陀螺零偏误差。

进一步展开叉积项：
\mathbf{\Omega} {ib}^b \times \boldsymbol{\phi} =
\begin{bmatrix}
0 & -\omega_z & \omega_y \
\omega_z & 0 & -\omega_x \
-\omega_y & \omega_x & 0
\end{bmatrix}
\boldsymbol{\phi}
\triangleq [\mathbf{\omega} {ib}^b]_\times \boldsymbol{\phi}

因此姿态误差方程可写成线性形式：
\dot{\boldsymbol{\phi}} = -[\mathbf{\omega} {ib}^b] \times \boldsymbol{\phi} + \mathbf{C}_b^n (\boldsymbol{\eta}_g - \mathbf{b}_g)
其中 $\boldsymbol{\eta}_g$ 为陀螺白噪声。

物理意义 ：姿态误差的变化由两部分驱动——一是载体旋转引起的坐标系变换效应，二是传感器噪声和零偏引入的真实扰动。

3.2.2 速度与位置误差微分方程的构建

速度误差方程

速度在导航系中的微分方程为：
\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
对其求误差形式，得到速度误差方程：
\delta \dot{\mathbf{v}}^n = \mathbf{C}_b^n \delta \mathbf{f}^b - [\mathbf{\omega} {ie}^n + \mathbf{\omega} {en}^n] \times \delta \mathbf{v}^n + [\delta \mathbf{g}^n] - [\mathbf{v}^n] \times (\boldsymbol{\omega} {ie}^n + \boldsymbol{\omega}_{en}^n) \times \boldsymbol{\phi}

忽略重力误差项，并代入加速度计误差模型 $\delta \mathbf{f}^b = \boldsymbol{\eta} a - \mathbf{b}_a$，得：
\delta \dot{\mathbf{v}}^n = \mathbf{C}_b^n (\boldsymbol{\eta}_a - \mathbf{b}_a) - [\mathbf{\omega} {cor}] \times \delta \mathbf{v}^n - [\mathbf{v}^n] \times \mathbf{\omega} {total} \times \boldsymbol{\phi}
其中 $\mathbf{\omega} {cor} = \mathbf{\omega} {ie}^n + \mathbf{\omega} {en}^n$。

位置误差方程

位置在地理坐标系下的变化率与速度有关：
\begin{aligned}
\delta \dot{L} &= \frac{\delta v_N}{R_N + h} \
\delta \dot{\lambda} &= \frac{\delta v_E}{(R_E + h)\cos L} \
\delta \dot{h} &= \delta v_D
\end{aligned}
其中 $R_N$、$R_E$ 为地球曲率半径。

线性化后可得：
\delta \dot{\mathbf{r}}^n = \mathbf{M}^{-1} \delta \mathbf{v}^n
其中 $\mathbf{M}$ 为当地尺度因子矩阵。

3.2.3 陀螺仪与加速度计误差的时间演化模型

如前所述，传感器零偏常建模为随机游走或一阶马尔可夫过程。以陀螺为例：

\dot{\mathbf{b}} g = -\frac{1}{\tau_g} \mathbf{b}_g + \boldsymbol{\eta} {bg}, \quad \boldsymbol{\eta} {bg} \sim \mathcal{N}(0, Q {bg})

同理，加速度计：
\dot{\mathbf{b}} a = -\frac{1}{\tau_a} \mathbf{b}_a + \boldsymbol{\eta} {ba}

这些方程直接构成状态向量中对应分量的动力学关系。

整合示例：15维系统F矩阵结构（表格）

子模块	维度	对应F矩阵块位置	内容说明
姿态误差	3×3	F(7:9,7:9)	-[ω_ib]×
速度误差 ← 姿态误差	3×3	F(4:6,7:9)	-[v^n]×[ω_total]×
速度误差自演化	3×3	F(4:6,4:6)	-[ω_cor]×
位置误差 ← 速度误差	3×3	F(1:3,4:6)	M⁻¹
陀螺零偏演化	3×3	F(10:12,10:12)	-I/τ_g
加速度计零偏演化	3×3	F(13:15,13:15)	-I/τ_a
传感器误差耦合	3×3	F(4:6,13:15), F(7:9,10:12)	C_b^n

此表展示了如何将各物理模块映射为系统矩阵 $ \mathbf{F} $ 的具体位置，便于编程实现。

3.3 离散化处理与系统状态转移矩阵构造

由于卡尔曼滤波在计算机中以离散时间方式运行，必须将连续时间模型 $\dot{\mathbf{x}} = \mathbf{F}\mathbf{x} + \mathbf{G}\mathbf{w}$ 转换为离散形式：
\mathbf{x} k = \mathbf{\Phi} {k/k-1} \mathbf{x} {k-1} + \mathbf{w} {k-1}
其中状态转移矩阵 $\mathbf{\Phi} = e^{\mathbf{F} \Delta t}$。

3.3.1 一阶泰勒展开法或矩阵指数法实现离散化

最常用的两种方法是：

1. 一阶近似法 ：
   $$
   \mathbf{\Phi} \approx \mathbf{I} + \mathbf{F} \Delta t
   $$
   简单快速，但仅适用于小 $\Delta t$ 或弱非刚性系统。

2. 矩阵指数法 ：
   $$
   \mathbf{\Phi} = \exp(\mathbf{F} \Delta t)
   $$
   更精确，尤其适合刚性强或高频动态场景。

MATLAB 提供了内置函数 expm() 计算矩阵指数：

dt = 0.01; % 采样周期（秒）
F_cont = compute_system_matrix(); % 用户自定义函数，返回F
Phi = expm(F_cont * dt);

参数说明 ：
- dt 应根据IMU输出频率设置（如100Hz → 0.01s）；
- expm 使用Pade逼近算法，数值稳定但计算开销较大；
- 建议对固定结构的F矩阵预先分析稀疏性以加速计算。

3.3.2 采样周期对模型精度的影响分析

采样周期 $\Delta t$ 的选择至关重要：

Δt 过大（>0.1s）	Δt 过小（<1ms）
截断误差显著	计算资源浪费
可能丢失高频动态特征	数值舍入误差积累
不满足采样定理	实时性压力大

建议原则：
- IMU更新频率为主导，一般取 1–10 ms；
- 若使用松组合（GPS 1Hz 更新），内部滤波仍保持高速率（如100Hz），仅在GPS时刻执行更新步；
- 对于紧组合，需同步处理伪距率，建议不低于50Hz。

3.3.3 MATLAB中离散化函数（如c2d）的应用技巧

MATLAB Control System Toolbox 提供 c2d 函数支持多种离散化方法：

sys_c = ss(F, G, [], []);           % 构建连续状态空间模型
sys_d = c2d(sys_c, dt, 'foh');      % 使用阶跃保持法（FOH）
Phi = sys_d.A;                      % 离散状态转移矩阵
Q_disc = sys_d.Q;                   % 离散化后的噪声协方差

支持的方法包括：
- 'zoh' : 零阶保持（默认，适用于输入恒定）
- 'foh' : 一阶保持（更平滑）
- 'tustin' : 双线性变换（频率响应匹配好）

推荐使用 'foh' 方法 ，尤其当过程噪声具有时间相关性时，能更好保留统计特性。

3.4 过程噪声协方差矩阵Q的设计策略

过程噪声协方差矩阵 $ \mathbf{Q} $ 描述了系统模型不确定性的强度，直接影响滤波器对新观测的信任程度。

3.4.1 各类误差源功率谱密度的量化方法

$ \mathbf{Q} $ 的构造依赖于各误差项的功率谱密度（PSD）：

误差源	PSD 参数表示	单位
陀螺白噪声	$ N_g $	(rad/s)/√Hz
陀螺随机游走	$ B_g $	rad/(s·√Hz)
加速度计白噪声	$ N_a $	(m/s²)/√Hz
加速度计随机游走	$ B_a $	m/(s²·√Hz)

例如，若陀螺角随机游走为 0.1 °/√hr，则换算为：
B_g = 0.1 \times \frac{\pi}{180} \div \sqrt{3600} \approx 4.85 \times 10^{-5} ~ \text{rad/s}^{3/2}

离散化后的噪声协方差可通过以下公式计算（随机游走模型）：

\mathbf{Q}_k = \int_0^{\Delta t} \mathbf{\Phi}(t) \mathbf{G} \mathbf{Q}_c \mathbf{G}^T \mathbf{\Phi}^T(t) dt

对于简单情况，可近似为：
\mathbf{Q} \approx \mathbf{G} \mathbf{Q}_c \mathbf{G}^T \Delta t

% 设定PSD参数
Ng = 0.01 * deg2rad(1);     % 陀螺ARW: 0.01 °/√s
Na = 1e-3;                  % 加速度计VRW: 0.001 m/s²/√Hz
Bg = Ng;                    % ARW = 白噪声积分
Ba = Na;

% 构造连续噪声协方差Qc
Qc = diag([
    (Ba)^2, (Ba)^2, (Ba)^2,           % 加速度计噪声
    (Bg)^2, (Bg)^2, (Bg)^2            % 陀螺噪声
]);

% 噪声分布矩阵G（作用于最后6维）
G = zeros(15, 6);
G(4:6, 1:3) = Cbn;         % 加速度计噪声影响速度
G(7:9, 4:6) = Cbn;         % 陀螺噪声影响姿态

% 离散化Q矩阵
Q_disc = G * Qc * G' * dt;

逻辑分析 ：
- Qc 是连续时间噪声强度；
- G 将噪声映射到状态空间；
- 最终 Q_disc 成为滤波器预测步中协方差增长的主要来源。

3.4.2 Q矩阵调参的经验准则与敏感性分析

调整 $ \mathbf{Q} $ 是滤波性能优化的关键手段：

调整方向	效果	风险
增大Q	提高跟踪能力，加快收敛	过滤效果变差，噪声放大
减小Q	平滑输出，抑制震荡	收敛慢，对模型误差敏感
分块调节	针对特定通道优化	需大量试验验证

经验法则 ：
- 初始设置按器件手册PSD参数；
- 通过残差分析（Innovation Sequence）判断Q是否合适；
- 若残差波动剧烈 → 增大对应通道Q；
- 若残差长期偏离零均值 → 检查模型偏差或增大相应状态Q。

敏感性分析流程图（Mermaid）

graph LR
    Start[开始调参] --> SetQ[设定初始Q矩阵]
    SetQ --> RunKF[运行卡尔曼滤波]
    RunKF --> CheckRes[分析残差序列]
    CheckRes -->|残差方差大| IncreaseQ[适度增大Q]
    CheckRes -->|残差收敛慢| DecreaseQ[减小Q或检查可观测性]
    IncreaseQ --> ReRun
    DecreaseQ --> ReRun
    ReRun --> RunKF
    CheckRes -->|符合白噪声特性| End[参数确定]

该闭环流程确保Q矩阵既能反映真实噪声水平，又能维持滤波器最优性能。

4. 测量模型设计与观测矩阵构建

在SINS/GPS组合导航系统中，卡尔曼滤波器的性能不仅依赖于精确的状态方程建模，更关键的是建立合理的 测量模型 。测量模型定义了系统状态误差与实际观测量之间的数学关系，是实现信息融合的核心环节之一。该模型通过观测矩阵 $ H $ 将高维状态空间映射到低维观测空间，从而为滤波器提供反馈修正依据。本章将围绕不同组合模式下的观测量选择、非线性观测方程的线性化处理、观测矩阵的结构构造以及测量噪声协方差阵 $ R $ 的动态设定展开深入探讨，重点解析其物理意义、数学推导过程及工程实现中的优化策略。

4.1 不同组合模式下的观测量选取

4.1.1 松组合中的位置/速度差值作为观测量

松组合（Loose Coupling）是最常见的SINS/GPS融合架构之一，其核心思想是分别独立运行惯导和GPS解算模块，然后以两者输出的位置和速度之差作为卡尔曼滤波的观测量。假设SINS解算出的位置为 $ \mathbf{r} {\text{ins}} $、速度为 $ \mathbf{v} {\text{ins}} $，而GPS提供的位置为 $ \mathbf{r} {\text{gps}} $、速度为 $ \mathbf{v} {\text{gps}} $，则观测量可表示为：

\mathbf{z} =
\begin{bmatrix}
\mathbf{r} {\text{ins}} - \mathbf{r} {\text{gps}} \
\mathbf{v} {\text{ins}} - \mathbf{v} {\text{gps}}
\end{bmatrix}
= \mathbf{H} \delta \mathbf{x} + \mathbf{v}_k

其中 $ \delta \mathbf{x} $ 是系统状态误差向量，$ \mathbf{v}_k $ 为零均值高斯白噪声。

这种设计的优势在于实现简单、计算开销小，并且对GPS信号中断具有较强的鲁棒性——当GPS失锁时，仅需关闭更新步即可退化为纯惯导工作模式。然而，松组合也存在明显的局限性：它无法利用原始伪距或载波相位信息，在城市峡谷或多路径严重的环境中定位精度下降明显。

下表对比了不同类型组合方式在典型城市环境下的表现特性：

组合模式	观测量类型	抗遮挡能力	计算复杂度	定位精度
松组合	位置/速度差	强	低	中等
紧组合	伪距/伪距率	中等	中	高
深组合	I/Q基带信号	弱	高	极高

从上表可见，松组合适用于对实时性和可靠性要求较高但精度需求适中的应用场景，如车载导航系统。

此外，还需注意 可观测度问题 。若长时间直线匀速运动，姿态角尤其是航向角的误差难以被有效观测，导致滤波器对该状态估计不准。因此，在松组合系统中常引入外部辅助源（如磁力计或轮速传感器）以增强系统整体可观测性。

4.1.2 紧组合中伪距与伪距率的直接使用

紧组合（Tight Coupling）突破了松组合仅使用位置/速度输出的限制，直接将GPS接收机输出的 卫星伪距 $ \rho_i $ 和 伪距率 $ \dot{\rho}_i $（即多普勒频移换算值）作为观测量输入至滤波器。设共有 $ n $ 颗可见卫星，则总观测量维度为 $ 2n $。

伪距的基本表达式为：
\rho_i = |\mathbf{r}_i - \mathbf{r}| + c(\delta t_u - \delta t^s) + I_i + T_i + \epsilon_i
其中：
- $ \mathbf{r}_i $：第 $ i $ 颗卫星位置；
- $ \mathbf{r} $：用户位置；
- $ c $：光速；
- $ \delta t_u $：接收机钟差；
- $ \delta t^s $：卫星钟差；
- $ I_i, T_i $：电离层与对流层延迟；
- $ \epsilon_i $：测量噪声。

由于该关系高度非线性，必须进行线性化处理。令真实状态估计为 $ \hat{\mathbf{x}} $，真实观测量为 $ h(\hat{\mathbf{x}}) $，则残差为：
\mathbf{z} = \boldsymbol{\rho}^{\text{meas}} - h(\hat{\mathbf{x}})

随后通过雅可比矩阵 $ H = \frac{\partial h}{\partial \mathbf{x}} $ 构造线性化后的观测方程：
\mathbf{z} \approx H \delta \mathbf{x} + \mathbf{v}

相较于松组合，紧组合的最大优势在于即使只有3~4颗卫星可用，仍可通过伪距联合定位完成滤波更新，显著提升了系统在遮挡环境下的可用性。同时，由于直接参与滤波的是原始测量值，避免了GPS内部EKF可能带来的信息损失。

以下为某典型场景下两种组合方式的可见卫星数与PDOP变化曲线示意图（使用mermaid流程图模拟趋势）：

graph TD
    A[时间序列] --> B[松组合: 卫星数<4时无更新]
    A --> C[紧组合: 利用所有可见卫星伪距]
    B --> D[定位中断风险增加]
    C --> E[持续提供观测信息]
    D --> F[导航精度骤降]
    E --> G[维持较高滤波稳定性]

由此可见，紧组合更适合于无人机、特种车辆等对连续性要求极高的应用场合。

4.1.3 观测冗余性与可观测度分析

观测冗余性是指系统能够提供的独立观测量数量相对于待估状态变量的数量是否充足。在SINS/GPS系统中，状态维数通常在15~21维之间（含位置、速度、姿态误差、陀螺零偏、加速度计零偏等），而每个可见卫星贡献两个标量观测（伪距+伪距率），故至少需要8颗卫星才能保证基本可观测性。

为了定量评估系统的 可观测度 ，常用方法包括：

1. 可观测度矩阵分析法 ：构造系统可观测度矩阵 $ \mathcal{O} = [H^T, (HF)^T, (HF^2)^T, …, (HF^{n-1})^T]^T $，检查其秩是否等于状态维数。
2. 奇异值分解（SVD）法 ：对滤波过程中累积的 $ H $ 矩阵做SVD，观察最小奇异值的变化趋势，判断某些状态是否长期不可观。
3. 后验残差分析法 ：监测新息（Innovation）序列的统计特性，若长期偏离零均值或方差异常增大，说明对应方向缺乏有效观测。

例如，考虑一个包含17维状态的系统，在静止启动阶段仅有4颗卫星可视，此时可观测度矩阵秩仅为10，表明姿态角和传感器零偏未能充分激励，滤波器收敛缓慢。解决办法包括：

• 增加机动动作（如转弯、加速）以激发系统动态响应；
• 引入地磁或里程计辅助观测；
• 使用自适应滤波调整 $ Q $ 或 $ R $ 矩阵以提升敏感状态的权重。

综上所述，合理选择观测量不仅要考虑当前硬件支持能力，还需结合任务环境动态调整融合策略，确保关键状态始终处于良好可观测状态。

4.2 线性化测量方程的推导过程

4.2.1 从非线性观测关系到小信号近似的转换

在紧组合系统中，伪距与用户位置之间呈非线性几何关系。设第 $ i $ 颗卫星的位置为 $ \mathbf{r}_i $，用户真实位置为 $ \mathbf{r} $，则理论伪距为：

\rho_i(\mathbf{r}) = |\mathbf{r}_i - \mathbf{r}| + c \cdot \delta t_u + b_i

其中 $ b_i $ 包括卫星钟差、大气延迟等已知或可模型化的偏差。

令当前最优估计为 $ \hat{\mathbf{r}}, \hat{\delta t}_u $，真实状态为 $ \mathbf{r} = \hat{\mathbf{r}} + \delta \mathbf{r}, \delta t_u = \hat{\delta t}_u + \delta b $，采用泰勒展开至一阶项：

\rho_i(\mathbf{r}) \approx \rho_i(\hat{\mathbf{r}}, \hat{\delta t} u) + \frac{\partial \rho_i}{\partial \mathbf{r}} \bigg| {\hat{\mathbf{r}}} \delta \mathbf{r} + \frac{\partial \rho_i}{\partial \delta t_u} \delta b

梯度项为：
\frac{\partial \rho_i}{\partial \mathbf{r}} = -\frac{\mathbf{r}_i - \hat{\mathbf{r}}}{|\mathbf{r}_i - \hat{\mathbf{r}}|} = -\mathbf{e}_i
即指向第 $ i $ 颗卫星的视线单位向量的反方向。

于是有：
\delta \rho_i = \rho_i^{\text{meas}} - \rho_i^{\text{pred}} \approx -\mathbf{e}_i^T \delta \mathbf{r} + c \cdot \delta b + v_i

这便是线性化后的伪距观测方程。类似地，伪距率的线性化形式为：
\delta \dot{\rho} i \approx -\mathbf{e}_i^T \delta \mathbf{v} + \mathbf{e}_i^T (\boldsymbol{\omega} {ie}^n \times \delta \mathbf{r}) + c \cdot \delta \dot{b} + w_i
其中包含了地球自转角速度引起的交叉耦合项。

上述推导基于“小误差”假设，即状态偏差 $ \delta \mathbf{x} $ 远小于系统尺度，否则高阶项不可忽略，需频繁重初始化参考点。

4.2.2 输出校正量与状态误差之间的映射关系

在扩展卡尔曼滤波（EKF）框架下，测量更新的目标是根据新息 $ \mathbf{y} = \mathbf{z} - h(\hat{\mathbf{x}}^-) $ 计算卡尔曼增益 $ K $，进而修正状态误差估计：

\delta \mathbf{x}^+ = K \mathbf{y}

这里的 $ \delta \mathbf{x} $ 并非绝对状态，而是相对于标称轨迹的小扰动。最终通过如下方式更新主状态：

\mathbf{x}^+ = \mathbf{x}^- \oplus \delta \mathbf{x}^+

其中 $ \oplus $ 表示李群上的更新操作（如SO(3)上的旋转向量叠加）。

观测模型的本质就是建立从状态误差到预测观测误差的线性映射：
\mathbf{z} = H \delta \mathbf{x} + \mathbf{v}

举例说明，若状态向量定义为：
\delta \mathbf{x} = [\delta \mathbf{r}^T, \delta \mathbf{v}^T, \boldsymbol{\phi}^T, \delta \mathbf{b}_g^T, \delta \mathbf{b}_a^T, \delta b, \delta \dot{b}]^T
共17维，则对于每一颗卫星，其对应的 $ H $ 子块为：

状态分量	在伪距行中的系数	在伪距率行中的系数
$ \delta \mathbf{r} $	$ -\mathbf{e}_i^T $	$ -\mathbf{e} i^T \boldsymbol{\omega} {ie}^\times $
$ \delta \mathbf{v} $	0	$ -\mathbf{e}_i^T $
$ \boldsymbol{\phi} $	$ (\mathbf{C}_b^n \mathbf{a})^T \mathbf{e}_i $	$ (\mathbf{C} b^n \boldsymbol{\omega} {ib}^b)^T \mathbf{e}_i $
$ \delta b $	$ c $	$ c $
其他	0	0

注：$ \mathbf{C} b^n $ 为当前姿态矩阵，$ \mathbf{a}, \boldsymbol{\omega} {ib}^b $ 分别为比力和角速度的机体坐标表示。

此表清晰展示了各状态分量如何影响观测量，是编写观测矩阵生成函数的重要依据。

4.3 观测矩阵H的构造方法

4.3.1 H矩阵的结构形式及其物理含义

观测矩阵 $ H \in \mathbb{R}^{m \times n} $ 是连接状态空间与观测空间的桥梁，其每一行代表一个观测量对各个状态变量的敏感程度，每一列表示某一状态变量对所有观测量的影响。

以紧组合为例，若有 $ N $ 颗可见卫星，每颗贡献伪距和伪距率两项观测，则 $ m = 2N $；若状态维数 $ n = 17 $，则 $ H \in \mathbb{R}^{2N \times 17} $。

具体结构如下所示：

H =
\begin{bmatrix}
-\mathbf{e}_1^T & \mathbf{0}^T & (\mathbf{C}_b^n \mathbf{a}_1)^T \mathbf{e}_1 & \cdots & c \
\mathbf{0}^T & -\mathbf{e}_1^T & (\mathbf{C}_b^n \boldsymbol{\omega}_1)^T \mathbf{e}_1 & \cdots & c \
\vdots & \vdots & \vdots & \ddots & \vdots \
-\mathbf{e}_N^T & \mathbf{0}^T & (\mathbf{C}_b^n \mathbf{a}_N)^T \mathbf{e}_N & \cdots & c \
\mathbf{0}^T & -\mathbf{e}_N^T & (\mathbf{C}_b^n \boldsymbol{\omega}_N)^T \mathbf{e}_N & \cdots & c \
\end{bmatrix}

其中前两列分别为位置误差和速度误差的投影方向，第三列为姿态误差通过当前加速度和角速度调制后对观测的影响，最后一列为钟差相关项。

物理意义上，$ H $ 实际反映了“几何构型”对状态估计精度的制约。例如，当所有卫星集中在天空一侧时，$ H $ 矩阵趋于病态，导致水平位置估计不准（表现为HDOP值升高）。因此，$ H $ 矩阵的质量直接影响滤波器的稳定性和收敛速度。

4.3.2 分块矩阵表示与程序实现优化

在实际编程中，建议采用 分块构造法 生成 $ H $ 矩阵，提高代码可读性和维护性。以下为MATLAB伪代码示例：

function H = build_observation_matrix(ins_state, sat_positions, sat_velocities)
    % 输入：
    %   ins_state: 当前INS状态结构体（含位置、速度、姿态、bias等）
    %   sat_positions: N x 3, 卫星ECEF坐标
    %   sat_velocities: N x 3, 卫星速度
    N = size(sat_positions, 1);
    H = zeros(2*N, 17);  % 假设17维状态
    pos_ins = ins_state.position;
    vel_ins = ins_state.velocity;
    Cbn = euler_to_dcm(ins_state.euler);  % 姿态矩阵
    af = ins_state.accel;                 % 比力
    omega = ins_state.gyro;               % 角速度
    for i = 1:N
        % 计算视线向量
        delta_r = sat_positions(i,:) - pos_ins;
        range = norm(delta_r);
        ei = delta_r / range;
        delta_v = sat_velocities(i,:) - vel_ins;
        edot = delta_v / range - ei * (ei' * delta_v) / range;
        % 伪距行
        H(2*i-1, 1:3) = -ei;                    % 位置误差
        H(2*i-1, 7:9) = cross(af, ei)';         % 姿态误差（Phi角模型）
        H(2*i-1, 16) = 1;                       % 接收机钟差（单位：米）
        % 伪距率先验行
        H(2*i, 4:6) = -ei;                      % 速度误差
        H(2*i, 7:9) = cross(omega, ei)';        % 姿态变化率影响
        H(2*i, 17) = 1;                         % 钟漂
    end
end

代码逻辑逐行解读：

• line 6 : 初始化 $ H $ 为全零矩阵，大小为 $ 2N \times 17 $。
• line 10–13 : 提取当前INS解算结果中的关键变量。
• line 17–18 : 计算第 $ i $ 颗卫星的视线单位向量 $ \mathbf{e}_i $，这是几何灵敏度的基础。
• line 23 : 伪距对位置误差的偏导为负视线方向。
• line 24 : 利用叉积 $ \mathbf{C}_b^n \mathbf{a} \times \mathbf{e}_i $ 表示姿态误差对伪距的影响（源于坐标系旋转）。
• line 25 : 钟差以米为单位等效加入（乘以光速前的数值）。
• line 29–31 : 类似处理伪距率项，速度误差直接影响多普勒，姿态变化通过角速度调制。

该函数可在每次滤波更新前调用，动态生成最新的 $ H $ 矩阵，确保模型一致性。

4.4 测量噪声协方差阵R的设定与自适应调整

4.4.1 GPS定位精度与PDOP值的关系建模

测量噪声协方差矩阵 $ R $ 描述了观测量的不确定性。在理想情况下，若所有卫星观测独立且噪声同分布，则 $ R $ 为对角阵，对角元为各通道的方差。

但现实中，GPS测量精度受多种因素影响，主要包括：

• 卫星几何分布（DOP）
• 信噪比（C/N₀）
• 多路径效应
• 大气延迟残余误差

经验公式常将伪距标准差建模为：
\sigma_{\rho,i} = \sigma_0 \cdot \text{WF} \cdot \sqrt{\text{GDOP} i}
或更实用的形式：
\sigma {\rho,i} = a + \frac{b}{\sin \theta_i + c}
其中 $ \theta_i $ 为卫星仰角，参数 $ a,b,c $ 可通过实测数据拟合获得。

例如，典型取值为：
- $ a = 3 $ m（底层噪声）
- $ b = 10 $ m
- $ c = 0.1 $

据此可构建对角阵 $ R $：
R = \text{diag}([\sigma_{\rho,1}^2, \sigma_{\dot{\rho},1}^2, \dots, \sigma_{\rho,N}^2, \sigma_{\dot{\rho},N}^2])

下表列出不同仰角下的噪声标准差估算：

仰角（°）	伪距噪声（m）	伪距率噪声（m/s）
10	12.3	0.15
30	5.8	0.08
60	3.5	0.05
90	3.0	0.04

这一表格可用于在线调整 $ R $ 矩阵，使滤波器在低仰角卫星存在时自动降低其权重。

4.4.2 基于环境感知的R阵动态修正策略

固定 $ R $ 矩阵容易导致滤波器在复杂环境下性能下降。为此，提出一种 自适应 $ R $ 调整算法 ，流程如下：

graph LR
    A[获取每颗卫星仰角和C/N₀] --> B{是否低于阈值?}
    B -- 是 --> C[增大对应R元素]
    B -- 否 --> D[按正常模型设置]
    C --> E[融合高楼检测标志]
    D --> F[输出最终R矩阵]
    E --> F

具体实现步骤：

1. 实时采集每颗卫星的SNR与仰角 ；
2. 判断是否处于城市峡谷区域 （可通过相邻卫星仰角差异大、平均SNR低识别）；
3. 对低仰角或低SNR卫星赋予更大噪声方差 ；
4. 可结合地图匹配或视觉SLAM输出可信度因子进一步加权 。

MATLAB片段示例：

function R = adaptive_R_matrix(snr_dbhz, elevation_deg, in_urban_area)
    N = length(snr_dbhz);
    R = zeros(2*N, 2*N);
    base_sigma_rho = 3.0;
    base_sigma_dot = 0.05;
    for i = 1:N
        % 根据仰角缩放
        sigma_factor = 1 + 10 * exp(-0.2 * elevation_deg(i));
        % 根据SNR衰减
        if snr_dbhz(i) < 38
            sigma_factor = sigma_factor * 1.5;
        end
        % 城市场景惩罚
        if in_urban_area
            sigma_factor = sigma_factor * 1.8;
        end
        R(2*i-1, 2*i-1) = (base_sigma_rho * sigma_factor)^2;
        R(2*i, 2*i) = (base_sigma_dot * sigma_factor)^2;
    end
end

参数说明：

• snr_dbhz : 接收机报告的载噪比（dB-Hz），典型健康值 > 40；
• elevation_deg : 卫星仰角，用于修正多路径敏感性；
• in_urban_area : 布尔标志，指示是否进入高楼密集区；
• sigma_factor : 综合放大系数，体现环境恶劣程度。

这种方法能显著提升滤波器在动态环境下的鲁棒性，尤其在GPS短暂失效前后保持平稳过渡。

5. 卡尔曼滤波预测与更新步骤实现

5.1 滤波初始化与参数配置流程

在启动卡尔曼滤波器之前，合理的初始化是确保滤波性能稳定、快速收敛的前提。初始状态向量 $\hat{x}_0$ 通常基于SINS/GPS系统的初始对准结果设定，包括位置误差、速度误差、姿态角误差（如东向、北向、天向的失准角），以及陀螺零偏和加速度计零偏等传感器误差项。

例如，在静止启动条件下，若GPS提供的初始位置精度为±3米（95%置信度），可将位置误差初始值设为0，协方差对应设置为：
P_{0}(p) = \text{diag}([3^2, 3^2, 3^2])
类似地，速度误差初值取0，假设GPS速度输出噪声标准差为0.1 m/s，则：
P_{0}(v) = \text{diag}([0.1^2, 0.1^2, 0.1^2])
姿态误差方面，若采用粗对准方法，典型失准角约为1°~5°，因此：
P_{0}(\theta) = \text{diag}([(5^\circ)^2, (5^\circ)^2, (5^\circ)^2])
对于惯性器件偏差，其初始不确定性较大，但可通过历史数据或标定经验值进行估计。例如，光纤陀螺零偏先验标准差设为0.01°/h，加速度计零偏设为1 mg，则相应协方差分块按单位换算后填入 $P_0$。

初始协方差矩阵 $P_0$ 的结构如下表所示（以15维状态为例）：

状态变量	维度	初始方差设置依据
位置误差 (m)	3	GPS定位精度
速度误差 (m/s)	3	GPS测速噪声
姿态误差 (rad)	3	对准误差统计
陀螺零偏 (rad/s)	3	器件手册或标定
加速度计零偏 (m/s²)	3	静态零偏测试

此外，还需设定过程噪声协方差矩阵 $Q$ 和测量噪声协方差矩阵 $R$ 的初始值，并判断滤波是否进入稳态。一种常见的收敛判据是监测残差平方和（RSS）的变化率小于阈值，例如连续10个周期内变化小于5%。

在实际系统中，常引入“启动保护机制”，即前若干秒内不反馈校正至SINS解算模块，避免因初始发散导致导航崩溃。

5.2 预测步的MATLAB代码实现

预测步是卡尔曼滤波递归结构的第一阶段，完成从 $k-1$ 时刻到 $k$ 时刻的状态与协方差推演。

核心公式如下：
\hat{x} {k|k-1} = F {k-1} \hat{x} {k-1|k-1}
P {k|k-1} = F_{k-1} P_{k-1|k-1} F_{k-1}^T + Q_{k-1}

其中，$F$ 为状态转移矩阵，可通过矩阵指数法由连续系统矩阵 $A$ 离散化得到，如使用 MATLAB 函数 c2d 实现：

% 示例：离散化连续动力学矩阵 A，采样周期 T
T = 0.1; % 10Hz 更新频率
sys_c = ss(A, zeros(size(A,1),1), [], []); % 构造连续状态空间模型
sys_d = c2d(sys_c, T, 'zoh'); % 零阶保持离散化
F = sys_d.A; % 获取离散状态转移矩阵

状态预测代码实现示例：

function [x_pred, P_pred] = predict_step(x_prev, P_prev, F, Q)
    % 输入：
    %   x_prev: 上一时刻最优状态估计 (n x 1)
    %   P_prev: 上一时刻协方差阵 (n x n)
    %   F: 状态转移矩阵 (n x n)
    %   Q: 过程噪声协方差阵 (n x n)
    % 输出：
    %   x_pred: 预测状态 (n x 1)
    %   P_pred: 预测协方差 (n x n)

    x_pred = F * x_prev;
    P_pred = F * P_prev * F' + Q;

    % 数值稳定性保障：强制对称化
    P_pred = (P_pred + P_pred') / 2;
end

为防止协方差矩阵在迭代中出现非正定或数值溢出，建议加入Cholesky分解检查或单位根监控。同时，稀疏矩阵存储可用于高维系统以提升运算效率。

5.3 更新步的完整算法执行

更新步利用当前时刻观测信息修正预测结果，实现最优估计。

关键公式包括：

• 卡尔曼增益计算 ：
  $$
  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})
  $$

• 协方差更新 ：
  $$
  P_{k|k} = (I - K_k H_k) P_{k|k-1}
  $$

MATLAB 实现片段如下：

function [x_upd, P_upd, residual] = update_step(x_pred, P_pred, z, H, R)
    % 计算残差
    residual = z - H * x_pred;

    % 计算创新协方差 S
    S = H * P_pred * H' + R;

    % 求解卡尔曼增益（推荐使用求解器而非显式逆）
    K = (S \ (H * P_pred))'; % 数值更稳定

    % 状态更新
    x_upd = x_pred + K * residual;

    % 协方差更新（Joseph form 更稳定）
    ImKH = eye(size(K,1)) - K * H;
    P_upd = ImKH * P_pred * ImKH' + K * R * K';

    % 强制对称
    P_upd = (P_upd + P_upd') / 2;
end

残差序列可用于健康监测。理想情况下，残差应呈零均值白噪声特性。通过绘制残差及其3σ边界，可直观判断是否存在模型失配或异常观测。

下表展示了某仿真场景下的前10个历元残差统计（单位：m）：

历元	东向残差	北向残差	天向残差	是否超限
1	0.12	-0.08	0.31	否
2	-0.45	0.23	-0.11	否
3	0.67	0.51	0.89	是（天向）
4	-0.21	-0.33	-0.15	否
5	0.09	0.17	0.22	否
6	-0.55	-0.61	-0.73	是（三向）
7	0.18	0.24	0.11	否
8	0.33	-0.41	0.27	否
9	-0.29	0.15	-0.35	否
10	0.41	0.52	0.63	是（北、天）

当残差持续超限时，应触发自适应调整机制，动态增大 $R$ 或启用野值剔除逻辑。

5.4 扩展卡尔曼滤波（EKF）在非线性系统中的集成

当系统存在显著非线性（如姿态四元数传播、伪距直接建模），需采用扩展卡尔曼滤波（EKF）。其核心是在每个时间步对非线性函数局部线性化，依赖雅可比矩阵实现近似。

具体流程如下：
1. 使用四元数积分更新姿态；
2. 构造非线性状态转移函数 $f(x)$ 和观测函数 $h(x)$；
3. 在当前工作点计算雅可比矩阵：
$$
F_k = \left.\frac{\partial f}{\partial x}\right| {\hat{x} {k-1}}, \quad
H_k = \left.\frac{\partial h}{\partial x}\right| {\hat{x} {k|k-1}}
$$

以姿态误差传播为例，四元数微分方程为：
\dot{q} = \frac{1}{2} \Omega(\omega) q
对应的误差状态转移雅可比涉及角速度和方向余弦阵的导数。

在代码中，可用符号工具箱或中心差分法自动求解雅可比：

function J = jacobian_numerical(f, x, h)
    % 数值雅可比计算（中心差分）
    n = length(x);
    fx = f(x);
    J = zeros(length(fx), n);
    for i = 1:n
        dx_plus = x; dx_plus(i) = x(i) + h;
        dx_minus = x; dx_minus(i) = x(i) - h;
        J(:,i) = (f(dx_plus) - f(dx_minus)) / (2*h);
    end
end

EKF相较标准KF能更好处理大失准角、紧组合等复杂情形，但计算负担增加约30%-50%，且需注意线性化误差积累问题。

5.5 “killman”压缩包中代码结构解析与运行实践

项目“killman”为开源SINS/GPS组合导航仿真平台，目录结构清晰，主要包含以下模块：

killman/
├── data_loader.m         # 加载IMU和GPS原始数据（.mat或.csv）
├── kalman_filter_core/   # 核心滤波算法
│   ├── ekf_predict.m
│   ├── ekf_update.m
│   └── jacobian_calc.m
├── navigation/           # SINS机械编排解算
│   ├── strapdown_integration.m
│   └── attitude_update.m
├── config/               # 参数配置文件
│   └── filter_params.mat
├── plot_results.m        # 可视化轨迹、误差、残差曲线
└── main_sins_gps_ekf.m   # 主程序入口

主程序执行流程如下图所示（Mermaid格式）：

graph TD
    A[开始] --> B[加载IMU/GPS数据]
    B --> C[初始化滤波器状态与协方差]
    C --> D[循环处理每一帧数据]
    D --> E[调用SINS解算更新姿态/速度/位置]
    D --> F[执行EKF预测步]
    D --> G[构建H矩阵并执行更新步]
    G --> H[反馈校正至SINS]
    H --> I{是否结束?}
    I -- 否 --> D
    I -- 是 --> J[保存结果并绘图]

关键接口调用示例如下：

% 主循环片段
for k = 1:length(imu_data)
    dt = imu_data(k).dt;
    omega = imu_data(k).gyro;
    acc = imu_data(k).accel;

    % SINS解算
    [att, vel, pos] = strapdown_integration(att, vel, pos, omega, acc, dt);

    % EKF预测
    [x_pred, P_pred] = ekf_predict(x_est, P_cov, F_func, Q, dt);

    % 若有GPS更新
    if gps_valid(k)
        z = [gps_pos(k,:)' - pos; gps_vel(k,:)' - vel];
        H = build_observation_matrix(pos, att); 
        R = compute_R_from_pdop(pdop(k));

        [x_upd, P_upd, res] = ekf_update(x_pred, P_pred, z, H, R);
        x_est = x_upd;
        P_cov = P_upd;

        % 反馈校正
        apply_feedback_correction(@att, @vel, @pos, x_est(1:9));
    else
        x_est = x_pred;
        P_cov = P_pred;
    end
end

常见调试问题包括：
- 协方差爆炸 → 检查 $Q$ 是否过大或 $F$ 不稳定；
- 滤波震荡 → 查看残差相关性，可能需重新设计 $H$ 矩阵；
- 初始化失败 → 增加对准时间或启用静基座粗对准。

性能优化建议：
- 使用 spdiags 和稀疏矩阵加速大规模 $P$ 运算；
- 将雅可比预计算缓存以减少重复求导；
- 采用平方根滤波（SR-EKF）提升数值鲁棒性。

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

简介：卡尔曼滤波是一种高效的递归滤波算法，广泛应用于动态系统状态估计中。在捷联惯导（SINS）与GPS的组合导航系统中，通过融合SINS连续高频率输出与GPS高精度定位的优势，卡尔曼滤波有效抑制传感器噪声与误差累积，提升导航系统的精度与鲁棒性。本文介绍基于MATLAB平台实现的SINS/GPS组合导航系统中卡尔曼滤波的设计与应用，涵盖状态建模、测量更新、预测校正流程及扩展卡尔曼滤波（EKF）处理非线性问题的方法，并结合“killman”压缩包中的代码实例，帮助理解其在实际工程中的部署过程。

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