cc的记事本
阅读_卡尔曼滤波
2025-12-01
论文阅读
00

目录

基于扩展卡尔曼滤波的视觉惯性融合定位详解
一、背景与问题
1.1 核心问题
二、符号定义与坐标系（ 3.1节）
2.1 目的
2.2 关键符号定义
三、相机与IMU联合标定（3.2节）
3.1 目的
3.2 标定内容
四、扩展卡尔曼滤波原理（3.3.1节）
4.1 目的
4.2 EKF算法流程
4.2.1 系统模型
4.2.2 预测步骤
4.2.3 更新步骤- 当相机图像到达时
五、小扰动模型（3.3.2节）
5.1 目的
5.2 李群与李代数
5.3 小扰动模型
问题的本质
基本思想
数学表示
无人机旋转估计
六、无人机状态描述（3.3.3节）
6.1 目的
6.2 IMU状态向量
6.3 相机状态向量
6.4 整体状态向量
6.5 意义
七、误差状态方程
7.1 目的
7.2 误差状态定义
7.3 运动学建模
7.4 连续时间误差状态方程
7.5 意义
八、状态传播与离散化
8.1 目的
8.2 四元数微分方程
8.3 龙格-库塔4阶方法
8.4 四元数更新
8.5 误差协方差传播
8.6 意义
九、观测模型（3.3.4节）
9.1 目的
9.2 视觉观测模型
9.3 观测模型
9.4 意义
十、雅可比矩阵计算
10.1 目的
10.2 链式求导法则
10.3 位置导数
10.4 旋转导数（小扰动模型）
10.5 全局雅可比矩阵
10.6 意义
十一、EKF更新过程
11.1 完整流程
11.2 更新公式回顾
11.3 意义
十二、整体系统总结
12.1 解决的问题
12.2 技术亮点
12.3 应用价值
十三、关键数学概念总结
13.1 坐标系变换
13.2 李群与李代数
13.3 状态估计
13.4 数值方法
十四、实际实现考虑
14.1 计算效率
14.2 数值稳定性
14.3 鲁棒性
结语

基于扩展卡尔曼滤波的视觉惯性融合定位详解

一、背景与问题

1.1 核心问题

问题描述：

  • 无人机需要精确跟踪目标，但仅依赖GNSS定位存在严重局限性
  • 在复杂或受限环境下（如室内、城市峡谷、信号干扰区域），GNSS信号可能失效或严重受干扰
  • 仅依赖GNSS的无人机无法完成自主跟踪任务

解决方案： 提出基于扩展卡尔曼滤波（EKF）的视觉惯性紧耦合定位方案，融合：

  • 视觉信息：从标记检测得到的几何约束
  • 惯性信息：IMU提供的加速度和角速度数据

核心目标： 实时估计目标与无人机之间的相对位置与姿态，为后续轨迹跟踪提供关键的状态估计支持

二、符号定义与坐标系（ 3.1节）

2.1 目的

解决的问题：

  • 不同传感器输出数据在不同坐标系中
  • 需要统一的坐标变换规则才能进行数据融合

2.2 关键符号定义

符号含义作用
WW世界坐标系全局参考系
oo目标坐标系目标标记的参考系
cc相机坐标系相机成像参考系
iiIMU坐标系惯性测量单元参考系
TABT_{AB}从B到A的刚体变换坐标系转换
pic,qicp_i^c, q_i^c相机在IMU系下的位置和四元数外参标定结果
RicR_i^c从相机到IMU的旋转矩阵外参标定结果
x^\hat{x}估计值状态估计
C(.)C_{(.)}四元数转旋转矩阵坐标变换工具
  • 统一语言：为后续所有推导提供一致的符号体系
  • 坐标对齐：确保不同传感器数据能在统一框架下处理
  • 数学基础：为后续的坐标变换和状态估计奠定基础

三、相机与IMU联合标定（3.2节）

3.1 目的

核心问题：

  • 相机和IMU安装在不同位置，有不同的坐标系
  • 需要知道它们之间的相对位置和姿态（外参）
  • 两者采样频率不同，存在时间同步问题

3.2 标定内容

空间标定：

  • 旋转矩阵 RicR_i^c：相机相对于IMU的旋转
  • 平移向量 picp_i^c：相机相对于IMU的位置

时间标定：

  • 时间延迟 Δt\Delta t：补偿相机和IMU之间的时间差

常用工具：

  • Kalibr：支持多种相机和IMU类型的联合标定平台

场景设定

无人机状态：

  • 位置：p=[x,y,z]Tp = [x, y, z]^T（世界坐标系）
  • 速度：v=[vx,vy,vz]Tv = [v_x, v_y, v_z]^T
  • 姿态：qq（四元数，表示朝向）
  • IMU零偏：ba,bwb_a, b_w（加速度计和陀螺仪零偏）

传感器：

  • IMU：100Hz，提供加速度ama_m和角速度wmw_m
  • 相机：30Hz，看到地面标记点，得到像素坐标

目标： 融合IMU和相机数据，估计无人机的位置、速度、姿态

四、扩展卡尔曼滤波原理（3.3.1节）

4.1 目的

解决的问题：

  • 经典卡尔曼滤波只能处理线性系统
  • 无人机运动模型和观测模型都是非线性的
  • 需要将非线性系统线性化处理

4.2 EKF算法流程

初始化（k=0k=0时刻）

状态： x^0=[p0,v0,q0,ba0,bw0]T\hat{x}0 = [p_0, v_0, q_0, b_{a0}, b_{w0}]^T

协方差： P0=diag([σp2,σv2,σq2,σba2,σbw2])P_0 = \text{diag}([\sigma_p^2, \sigma_v^2, \sigma_q^2, \sigma_{ba}^2, \sigma_{bw}^2])

含义： 初始位置、速度、姿态、零偏的估计及其不确定性

4.2.1 系统模型

状态预测模型：

xk=f(xk−1,uk−1)+wk−1x_k = f(x_{k-1}, u_{k-1}) + w_{k-1}

  • 含义：
    • xkx_k：kk时刻的真实状态（未知）
    • f(xk−1,uk−1)f(x_{k-1}, u_{k-1})：用上一时刻状态和控制输入，通过运动模型计算当前状态
    • wk−1w_{k-1}：过程噪声（模型不完美、外界干扰等）

目的：描述状态如何随时间变化。

观测模型：

zk=h(xk)+vkz_k = h(x_k) + v_k

  • 含义：
    • zkz_k：传感器测量值
    • h(xk)h(x_k)：从状态到观测的映射（如相机将3D位置投影为2D像素）
    • vkv_k：观测噪声（传感器误差）
  • 目的：描述如何从状态得到观测。

4.2.2 预测步骤

1. 状态预测： x^k∣k−1=f(x^k−1∣k−1,uk−1)\hat{x}_{k|k-1} = f(\hat{x}_{k-1|k-1}, u_{k-1})

  • x^k∣k−1\hat{x}{k|k-1}：基于k−1k-1时刻信息，对kk时刻状态的预测
  • 用上一时刻的估计和控制输入，通过运动模型预测当前状态

目的：在没有新观测时，先给出预测。

输入：

  • 上一时刻状态：x^k−1∣k−1=[pk−1,vk−1,qk−1,ba,k−1,bw,k−1]T\hat{x}_{k-1|k-1} = [p_{k-1}, v_{k-1}, q_{k-1}, b_{a,k-1}, b_{w,k-1}]^T
  • IMU测量：uk−1=[am,wm]u_{k-1} = [a_m, w_m]（加速度和角速度）

输出：

  • x^k∣k−1=[pk,vk,qk,ba,k,bw,k]T\hat{x}_{k|k-1} = [p_k, v_k, q_k, b{a,k}, b_{w,k}]^T

2. 线性化状态转移函数： Fk−1=∂f∂x∣x=x^k−1∣k−1,u=uk−1∈Rn×nF_{k-1} = \frac{\partial f}{\partial x}\Big|_{x=\hat{x}_{k-1|k-1}, u=u_{k-1}} \in \mathbb{R}^{n \times n}

  • Fk−1F_{k-1}：状态转移函数ff在估计点x^k−1∣k−1\hat{x}_{k-1|k-1}处的雅可比矩阵
  • 将非线性ff在局部线性化，近似为f(x)≈f(x^)+F⋅(x−x^)f(x) \approx f(\hat{x}) + F \cdot (x - \hat{x})

为什么：系统模型ff和观测模型hh是非线性的，卡尔曼滤波的协方差传播需要线性模型；所以EKF在局部线性化，用雅可比矩阵近似非线性关系

3. 协方差预测： Pk∣k−1=Fk−1Pk−1∣k−1Fk−1T+Qk−1P_{k|k-1} = F_{k-1} P_{k-1|k-1} F_{k-1}^T + Q_{k-1}

  • Pk∣k−1P_{k|k-1}：预测状态的不确定性（协方差）
  • Fk−1Pk−1∣k−1Fk−1TF_{k-1} P_{k-1|k-1} F_{k-1}^T：通过线性化模型传播上一时刻的不确定性
  • Qk−1Q_{k-1}：过程噪声协方差（模型误差带来的不确定性）

目的：量化预测的不确定性，为后续融合提供权重依据。

等待相机观测

频率：

  • IMU：100Hz（每10ms一次）
  • 相机：30Hz（每33ms一次）

如果此时没有新的相机图像，继续用IMU预；如果有新的相机图像，进入更新步骤

4.2.3 更新步骤- 当相机图像到达时

1. 观测预测： z^k=h(x^k∣k−1)\hat{z}_k = h(\hat{x}_{k|k-1})

  • 用预测状态，通过观测模型计算期望的观测值

目的：与真实观测对比，得到残差。

输入：

  • 预测状态：x^k∣k−1=[pk,vk,qk,⋯ ]T\hat{x}_{k|k-1} = [p_k, v_k, q_k, \cdots]^T
  • 地面标记点位置：pmarkerwp_{marker}^w（世界坐标系）

观测模型：

  • 1、标记点在世界坐标系的位置：pmarkerwp_{marker}^w
  • 2、转换到相机坐标系：

pmarkerc=C(qcw)T(pmarkerw−pk)p_{marker}^c = C(q_c^w)^T (p_{marker}^w - p_k)，qcwq_c^w是相机相对于世界的姿态

  • 投影到像素坐标： z^k=[uv]=[fxpmarker,xcpmarker,zc+cxfypmarker,ycpmarker,zc+cy]\hat{z}k = \begin{bmatrix} u \\ v \end{bmatrix} = \begin{bmatrix} f_x \frac{p{marker,x}^c}{p_{marker,z}^c} + c_x \\ f_y \frac{p_{marker,y}^c}{p_{marker,z}^c} + c_y \end{bmatrix}

fx,fyf_x, f_y是焦距，cx,cyc_x, c_y是主点

输出：

  • z^k=[u,v]T\hat{z}k = [u, v]^T（预测的像素坐标）

基于预测状态，计算标记点应该在图像中的位置

残差计算

  • 相机实际测量：

zk=[uactual,vactual]Tz_k = [u_{actual}, v_{actual}]^T

  • 观测残差：

rk=zk−z^k=[uactual−uvactual−v]r_k = z_k - \hat{z}k = \begin{bmatrix} u_{actual} - u \\ v_{actual} - v \end{bmatrix}

  • 含义：残差大：预测偏差大，需要修正；残差小：预测较准

2. 线性化观测函数： Hk=∂h∂x∣x=x^k∣k−1∈Rl×nH_k = \frac{\partial h}{\partial x}\Big|_{x=\hat{x}_{k|k-1}} \in \mathbb{R}^{l \times n}

  • HkH_k：观测函数hh在预测点处的雅可比矩阵
  • 将非线性hh在局部线性化

目的：为计算卡尔曼增益和协方差更新提供线性近似。

3. 计算卡尔曼增益： Kk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1}

  • RkR_k：观测噪声协方差（相机像素误差）

卡尔曼增益的意义 直观理解：

  • 决定系统更信任预测值还是观测值

情况1：相机很准，IMU预测不准

  • RkR_k很小（相机误差小）
  • Pk∣k−1P_{k|k-1}很大（预测不确定）
  • 结果：KkK_k较大，更信任相机观测

情况2：相机不准，IMU预测较准

  • RkR_k很大（相机误差大）
  • Pk∣k−1P_{k|k-1}较小（预测较确定）
  • 结果：KkK_k较小，更信任IMU预测

目的：决定预测与观测的融合权重。

4. 状态更新： x^k∣k=x^k∣k−1+Kk(zk−z^k)\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k(z_k - \hat{z}_k)

  • x^k∣k\hat{x}{k|k}：融合后的状态估计
  • zk−z^kz_k - \hat{z}k：观测残差（实际观测与预测观测的差）
  • Kk(zk−z^k)K_k(z_k - \hat{z}k)：用残差修正预测，修正量由KkK_k加权

目的：用观测修正预测，得到更准确的状态估计

  • 如果KkK_k较大（更信任相机），修正幅度大
  • 如果KkK_k较小（更信任IMU），修正幅度小

结果： x^k∣k=[pkupdated,vkupdated,qkupdated,⋯ ]T\hat{x}_{k|k} = [p_k^{updated}, v_k^{updated}, q_k^{updated}, \cdots]^T，融合后的状态估计，通常比单独使用IMU或相机更准确

5. 协方差更新： Pk∣k=(I−KkHk)Pk∣k−1P_{k|k} = (I - K_k H_k) P_{k|k-1}

  • Pk∣kP_{k|k}：更新后的不确定性
  • I−KkHkI - K_k H_k：融合后不确定性降低
  • 融合观测后，不确定性通常减小

目的：更新不确定性，为下一时刻提供基础

完整时间线示例

时刻 t0t_0：

  • 初始化状态和协方差

时刻 t1t_1（10ms后，IMU数据到达）：

  • 预测：用IMU数据预测状态
  • 更新协方差
  • 继续等待相机

时刻 t2t_2（20ms后，IMU数据到达）：

  • 预测：用IMU数据预测状态
  • 更新协方差
  • 继续等待相机

时刻 t3t_3（33ms后，相机图像到达）：

  • 预测：用IMU数据预测状态
  • 观测：用相机数据计算残差
  • 融合：计算卡尔曼增益，更新状态
  • 结果：得到融合后的状态估计

时刻 t4t_4（40ms后，IMU数据到达）： 预测：用IMU数据预测状态 继续等待相机

...

EKF在无人机IMU+相机融合中的工作方式：用IMU做高频预测，用相机做低频修正IMU累积误差，通过不确定性权重实现自适应融合

五、小扰动模型（3.3.2节）

5.1 目的

核心问题：

  • 无人机状态包含旋转和位姿，属于非欧几里得空间（李群SO(3)和SE(3)）
  • 传统欧几里得空间的简单加法不适用于旋转和位姿
  • 直接加法会破坏旋转的约束（单位四元数必须在单位球面上）

5.2 李群与李代数

李群（Lie Group）：

  • 同时具有群结构和流形结构
  • 可以表示连续可微的变换（如旋转、位姿）
  • 例子：SO(3)（旋转群）、SE(3)（刚体变换群）

李代数（Lie Algebra）：

  • 李群在单位元处的切空间
  • 具有欧几里得向量空间的性质
  • 可以进行加性建模和线性化分析

5.3 小扰动模型

问题的根源：旋转不能直接相加

问题的本质

普通数字（位置）：

  • 当前位置：p=[1,2,3]p = [1, 2, 3]
  • 想移动：Δp=[0.1,0.2,0.3]\Delta p = [0.1, 0.2, 0.3]
  • 新位置：pnew=p+Δp=[1.1,2.2,3.3]p_{new} = p + \Delta p = [1.1, 2.2, 3.3]

旋转（四元数）：

  • 当前旋转：q=[0.707,0.707,0,0]q = [0.707, 0.707, 0, 0]（绕Z轴转90度）
  • 想加一个小扰动：Δq=[0.1,0.1,0.1,0.1]\Delta q = [0.1, 0.1, 0.1, 0.1]
  • 直接相加：qnew=q+Δqq_{new} = q + \Delta q 会破坏单位约束

问题： 旋转不能直接相加，但EKF需要线性化，线性化需要“相加”的概念

基本思想

  • 将状态表示为参考状态加上小扰动
  • 使用指数映射将李代数元素映射回李群

第一步：在“切空间”中表示误差

  • 把误差表示成旋转向量 δθ\delta\theta（3维向量，可以相加）
  • 这个向量在“切空间”中，是欧几里得空间

第二步：映射回旋转群

  • 用指数映射把切空间的误差映射回旋转群
  • 得到的新旋转仍然是有效的旋转

类比：想象一个球面（旋转群）,在球面上某点的切平面（切空间/李代数）,在切平面上做加法，然后投影回球面

数学表示

基本的小扰动模型: R=exp⁡(ξ∧)R^R = \exp(\xi^\wedge)\hat{R}

其中：

  • R^\hat{R}：参考旋转（当前估计的旋转）
  • ξ\xi：扰动向量（6维：3个旋转 + 3个平移）
  • ξ∧\xi^\wedge：将向量转为反对称矩阵
  • exp⁡(ξ∧)\exp(\xi^\wedge)：指数映射，将切空间的扰动映射回旋转群
  • RR：扰动后的旋转（仍在旋转群中）

用切空间的扰动表示旋转的变化,通过指数映射保证结果仍是有效旋转

反对称矩阵： 对于向量ξ=(a,b,c)\xi=(a,b,c)： ξ∧=[0−cbc0−a−ba0]\xi^\wedge = \begin{bmatrix} 0 & -c & b \\ c & 0 & -a \\ -b & a & 0 \end{bmatrix}

将3维向量转为3×3反对称矩阵, 用于指数映射的计算

四元数的小扰动： R(q)=R(δq⊗q^)≈exp⁡(δθ∧)R(q^)R(q) = R(\delta q \otimes \hat{q}) \approx \exp(\delta\theta^\wedge)R(\hat{q})

  • q^\hat{q}：参考四元数
  • δq\delta q：四元数扰动
  • δθ\delta\theta：旋转向量（3维）
  • exp⁡(δθ∧)\exp(\delta\theta^\wedge)：将旋转向量转为旋转矩阵
  • 近似：当扰动很小时，四元数乘法近似等于指数映射
  • 四元数不能直接相加，旋转向量可以相加，用旋转向量近似四元数扰动

线性近似： R(q)P≈(I+δθ∧)R^PR(q)P \approx (I + \delta\theta^\wedge)\hat{R}P

线性化旋转操作:当扰动很小时，用线性近似代替非线性操作

EKF的问题：

需要线性化：f(x)≈f(x^)+F⋅(x−x^)f(x) \approx f(\hat{x}) + F \cdot (x - \hat{x})

但旋转不能直接相减：q−q^q - \hat{q} 没有意义

解决方案：

  • 主状态用四元数（保持约束）
  • 误差状态用旋转向量（可以相减）
  • 通过小扰动模型连接两者

无人机旋转估计

1.场景设定

无人机状态：

  • 位置：p=[1,2,3]p = [1, 2, 3]
  • 速度：v=[0.1,0.2,0.3]v = [0.1, 0.2, 0.3]
  • 旋转：q=[0.707,0.707,0,0]q = [0.707, 0.707, 0, 0]（绕Z轴转90度）

IMU测量：

  • 角速度：w=[0.1,0.05,0.02]w = [0.1, 0.05, 0.02]（弧度/秒）
  • 时间步： Δt=0.01\Delta t = 0.01 秒

相机观测（稍后到达）：

  • 实际观测：zk=[320,240]z_k = [320, 240]（像素坐标）
  • 预测观测：z^k=[315,245]\hat{z}_k = [315, 245]（基于预测状态）

2.预测步骤

  • 步骤1：主状态更新（非线性）

用IMU的角速度数据更新无人机的旋转： qk=qk−1⊗exp⁡(12wΔt)q_k = q_{k-1} \otimes \exp(\frac{1}{2}w \Delta t)

计算旋转增量： 12wΔt=12×[0.1,0.05,0.02]×0.01=[0.0005,0.00025,0.0001]\frac{1}{2}w \Delta t = \frac{1}{2} \times [0.1, 0.05, 0.02] \times 0.01 = [0.0005, 0.00025, 0.0001] 这是0.01秒内的旋转增量（旋转向量形式）

转换为四元数： exp⁡(12wΔt)\exp(\frac{1}{2}w \Delta t) 将旋转向量转换为四元数。当角度很小时，可以近似为： qincrement≈[1,0.0005,0.00025,0.0001]q_{increment} \approx [1, 0.0005, 0.00025, 0.0001]

四元数乘法： qk=qk−1⊗qincrementq_k = q_{k-1} \otimes q_{increment} 先应用当前旋转 qk−1q_{k-1}，再应用增量旋转 qincrementq_{increment}

结果： qk=[0.707,0.707,0.0001,0.00005]q_k = [0.707, 0.707, 0.0001, 0.00005]（近似值）

  • 步骤2：误差状态传播（线性）——目的： 预测误差状态如何随时间变化

在切空间中传播误差： Δθk=F⋅Δθk−1\Delta\theta_k = F \cdot \Delta\theta_{k-1}

其中 FF 是雅可比矩阵，在切空间中计算。

传播误差 = 误差如何从一个时刻传递到下一个时刻

当前时刻有误差，下一时刻误差会变化（可能增大、减小、或改变方向），这个过程就是误差传播

例子：IMU测量角速度有误差：wmeasured=wtrue+noisew_{measured} = w_{true} + noise，用这个测量值预测旋转，预测也会有误差，误差会累积和传播

场景：

估计的旋转：q^=[0.707,0.707,0,0]\hat{q} = [0.707, 0.707, 0, 0]（绕Z轴转90度）

真实的旋转：qtrue=[0.707,0.707,0.01,0.01]q_{true} = [0.707, 0.707, 0.01, 0.01]（稍微不同）

误差： Δθ=[0.01,0.01,0.01]\Delta\theta = [0.01, 0.01, 0.01]，可以相加

从预测误差切空间中使用线性运算，为了适应EKF的需要，直到用EKF去更新误差值，这之后误差值与预测值的"加法"就要在旋转群中完成

3.更新步骤（当相机观测到达时）

  • 步骤1：观测预测

用预测状态计算期望观测： z^k=h(q^k)\hat{z}_k = h(\hat{q}_k)

  • 步骤2：计算残差

rk=zk−z^kr_k = z_k - \hat{z}k

例：rk=zk−z^k=[320,240]−[315,245]=[5,−5]r_k = z_k - \hat{z}k = [320, 240] - [315, 245] = [5, -5]

  • 步骤3：在切空间中更新误差

Δθnew=Δθold+K⋅rk\Delta\theta_{new} = \Delta\theta_{old} + K \cdot r_k

K是卡尔曼增益，用观测残差修正IMU预测的误差，在切空间中线性更新： 因为误差状态是旋转向量，可以相加，而主状态是四元数，不能直接相加，因此在切空间中更新误差，再映射回主状态

  • 步骤4：映射回主状态（旋转群中的“加法”：误差值“加上”预测值）

qnew=exp⁡(Δθnew∧)⊗qoldq_{new} = \exp(\Delta\theta_{new}^\wedge) \otimes q_{old}

指数映射将切空间中的误差映射回旋转群中的主状态，但可能不是单位四元数

  • 步骤5：归一化

确保 qnewq_{new} 仍是单位四元数。

六、无人机状态描述（3.3.3节）

6.1 目的

核心任务：

  • 明确系统状态的建模
  • 定义完整的状态向量，包含所有需要估计的量

6.2 IMU状态向量

xIMU=[(pwi)T(vwi)T(qwi)T(bw)T(ba)T]Tx_{IMU} = \begin{bmatrix} (p_w^i)^T & (v_w^i)^T & (q_w^i)^T & (b_w)^T & (b_a)^T \end{bmatrix}^T

各分量含义：

  • pwi∈R3p_w^i \in \mathbb{R}^3：IMU在世界坐标系下的位置
  • vwi∈R3v_w^i \in \mathbb{R}^3：IMU在世界坐标系下的速度
  • qwiq_w^i：IMU在世界坐标系下的姿态（四元数）
  • bw∈R3b_w \in \mathbb{R}^3：陀螺仪零偏
  • ba∈R3b_a \in \mathbb{R}^3：加速度计零偏

6.3 相机状态向量

xc=[(pic)T(qic)T(pow)T(qow)T]Tx_c = \begin{bmatrix} (p_i^c)^T & (q_i^c)^T & (p_o^w)^T & (q_o^w)^T \end{bmatrix}^T

各分量含义：

  • pic,qicp_i^c, q_i^c：相机在IMU系下的位置与姿态（外参）
  • pow,qowp_o^w, q_o^w：世界坐标系到视觉框架坐标系的位置与姿态

6.4 整体状态向量

x=[xIMUTxcT]Tx = \begin{bmatrix} x_{IMU}^T & x_c^T \end{bmatrix}^T

维度： 30维向量

状态组成：

  • IMU状态：16维（3+3+4+3+3）
  • 相机状态：14维（3+4+3+4）

6.5 意义

  • 完整描述：涵盖了无人机定位所需的所有状态信息
  • 融合基础：为视觉和惯性数据融合提供统一的状态空间
  • 估计目标：明确了需要实时估计的所有变量

七、误差状态方程

7.1 目的

核心问题：

  • 所有姿态采用单位四元数表示，约束在单位球面上
  • 直接进行标准加法线性化在EKF中不适用
  • 需要引入误差状态的思想

7.2 误差状态定义

基本思想：

  • 主状态：使用四元数等非线性表示
  • 误差状态：使用小扰动（旋转向量）表示误差
  • 误差状态在欧几里得空间中，可以进行线性运算

IMU误差状态： x~IMU=[(Δpwi)T(Δvwi)T(δθwi)T(Δbw)T(Δba)T]T\tilde{x}_{IMU} = \begin{bmatrix} (\Delta p_w^i)^T & (\Delta v_w^i)^T & (\delta\theta_w^i)^T & (\Delta b_w)^T & (\Delta b_a)^T \end{bmatrix}^T

相机误差状态： x~c=[(Δpci)T(δθci)T(Δpow)T(δθow)T]T\tilde{x}_c = \begin{bmatrix} (\Delta p_c^i)^T & (\delta\theta_c^i)^T & (\Delta p_o^w)^T & (\delta\theta_o^w)^T \end{bmatrix}^T

误差状态关系： x~=x^−x\tilde{x} = \hat{x} - x

其中δθ\delta\theta表示旋转向量（小扰动）

7.3 运动学建模

IMU测量值处理： w^=wm−b^w\hat{w} = w_m - \hat{b}_w a^=am−b^a\hat{a} = a_m - \hat{b}_a

其中：

  • w^,a^\hat{w}, \hat{a}：角速度和加速度的估计值
  • wm,amw_m, a_m：角速度和加速度的测量值

7.4 连续时间误差状态方程

位置误差： Δp˙iw=Δviw\Delta \dot{p}_i^w = \Delta v_i^w

速度误差： Δv˙iw=−C(q^wi)(a^i)δθiw−C(q^wi)Δba−C(q^wi)na\Delta \dot{v}_i^w = -C(\hat{q}_w^i)(\hat{a}^i)\delta\theta_i^w - C(\hat{q}_w^i)\Delta b_a - C(\hat{q}_w^i)n_a

姿态误差： δθ˙iw=−(w^i)δθiw−Δbw−nw\delta\dot{\theta}_i^w = -(\hat{w}^i)\delta\theta_i^w - \Delta b_w - n_w

零偏误差： Δb˙w=nbw,Δb˙a=nba\Delta \dot{b}_w = n_{b_w}, \quad \Delta \dot{b}_a = n_{b_a}

外参误差（假设不变）： Δp˙ic=0,δq˙ic=0\Delta \dot{p}_i^c = 0, \quad \delta \dot{q}_i^c = 0

视觉框架误差（假设不变）： Δp˙ow=0,δq˙ow=0\Delta \dot{p}_o^w = 0, \quad \delta \dot{q}_o^w = 0

矩阵形式： x~˙=A~x~+Bn\dot{\tilde{x}} = \tilde{A} \tilde{x} + B n

其中：

  • A~\tilde{A}：30×24系统矩阵
  • BB：24×12输入矩阵
  • nn：噪声向量 n=[naT nbT nwT nbwT]Tn=[n_a^T \ n_b^T \ n_w^T \ n_{bw}^T]^T

7.5 意义

  • 线性化基础：误差状态在欧几里得空间，可以进行线性化
  • 保持约束：主状态（四元数）保持单位约束
  • 误差传播：可以准确描述状态不确定性的传播
  • 更新机制：误差更新后可以重置，保持主状态合理性

八、状态传播与离散化

8.1 目的

核心问题：

  • EKF预测阶段需要基于系统运动模型进行时间更新
  • 实际系统需要采用离散的时间观测和计算
  • 需要对连续时间的非线性模型进行数值积分离散化

8.2 四元数微分方程

四元数运动学： q˙=12Ω(w)q\dot{q} = \frac{1}{2}\Omega(w)q

Ω(w)\Omega(w)矩阵： Ω(w)=[−w∧w−wT0]\Omega(w) = \begin{bmatrix} -w^\wedge & w \\ -w^T & 0 \end{bmatrix}

这是一个4×4反对称矩阵，用于四元数微分。

8.3 龙格-库塔4阶方法

为什么使用RK4？

  • 系统非线性程度较高
  • RK4提供较高的数值精度
  • 适合实时系统

速度积分（RK4）： k1v=C(qt)⋅a^+gk_1^v = C(q_t) \cdot \hat{a} + g k2v=C(qt+Δt/2)⋅a^+gk_2^v = C(q_{t+\Delta t/2}) \cdot \hat{a} + g k3v=C(qt+Δt/2)⋅a^+gk_3^v = C(q_{t+\Delta t/2}) \cdot \hat{a} + g k4v=C(qt+Δt)⋅a^+gk_4^v = C(q_{t+\Delta t}) \cdot \hat{a} + g

位置积分（RK4）： k1p=vtk_1^p = v_t k2p=vt+Δt2k1vk_2^p = v_t + \frac{\Delta t}{2} k_1^v k3p=vt+Δt2k2vk_3^p = v_t + \frac{\Delta t}{2} k_2^v k4p=vt+Δtk3vk_4^p = v_t + \Delta t k_3^v

最终更新： vt+Δt=vt+Δt6(k1v+2k2v+2k3v+k4v)v_{t+\Delta t} = v_t + \frac{\Delta t}{6}(k_1^v + 2k_2^v + 2k_3^v + k_4^v) pt+Δt=pt+Δt6(k1p+2k2p+2k3p+k4p)p_{t+\Delta t} = p_t + \frac{\Delta t}{6}(k_1^p + 2k_2^p + 2k_3^p + k_4^p)

8.4 四元数更新

小角度近似： qt+Δt=δq⊗qtq_{t+\Delta t} = \delta q \otimes q_t

归一化： 四元数更新后需要归一化以保证四元数规范性（单位四元数约束）

8.5 误差协方差传播

状态转移矩阵： Φt=exp⁡(A~Δt)≈I+A~Δt+12A~2Δt2+16A~3Δt3\Phi_t = \exp(\tilde{A}\Delta t) \approx I + \tilde{A}\Delta t + \frac{1}{2}\tilde{A}^2\Delta t^2 + \frac{1}{6}\tilde{A}^3\Delta t^3

使用三阶泰勒展开近似指数矩阵。

协方差传播： Pt+Δt=ΦtPtΦtT+QtP_{t+\Delta t} = \Phi_t P_t \Phi_t^T + Q_t

协方差定义： Pt=E[ΔxΔxT]P_t = E[\Delta x \Delta x^T]

过程噪声协方差： Qt=∫0ΔtΦτBQcBTΦτTdτQ_t = \int_0^{\Delta t} \Phi_\tau B Q_c B^T \Phi_\tau^T d\tau

8.6 意义

  • 数值稳定性：RK4方法提供高精度数值积分
  • 实时性：离散化使得可以在实际系统中实现
  • 不确定性传播：准确刻画系统在预测阶段的状态不确定性变化
  • 计算效率：线性化误差状态传播，计算复杂度可控

九、观测模型（3.3.4节）

9.1 目的

核心任务：

  • 将视觉信息（标记点像素坐标）转换为对系统状态的观测
  • 建立观测模型，用于EKF的更新步骤

9.2 视觉观测模型

单个标记点的像素坐标： zi=Sledi=1[pcl]z[fx0cx0fycy]pclz_i = S_{led_i} = \frac{1}{[p_c^l]_z} \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \end{bmatrix} p_c^l

其中：

  • SlediS_{led_i}：第ii个LED的像素坐标
  • [⋅]z[\cdot]_z：取向量的zz坐标
  • pclp_c^l：LED标记在相机坐标系下的位置
  • fx,fyf_x, f_y：相机焦距
  • cx,cyc_x, c_y：相机主点坐标

LED在相机坐标系下的位置： pcl=(C(qcw)C(qwl)C(qlp))T{plp−[C(qcw)(C(qwl)plw+pwl)+pcw]}p_c^l = (C(q_c^w)C(q_w^l)C(q_l^p))^T \{p_l^p - [C(q_c^w)(C(q_w^l)p_l^w + p_w^l) + p_c^w]\}

这是一个复杂的坐标变换，涉及多个坐标系的转换。

9.3 观测模型

组合所有标记点： z^=h(x^)\hat{z} = h(\hat{x})

观测残差： r=z−h(x^)r = z - h(\hat{x})

线性化： r≈HΔx+nr \approx H \Delta x + n

其中：

  • HH：观测模型对误差状态的雅可比矩阵
  • nn：观测噪声

9.4 意义

  • 几何约束：视觉信息提供与目标标记点之间的几何约束
  • 状态更新：观测残差用于更新系统状态估计
  • 融合基础：将视觉信息纳入统一的滤波框架

十、雅可比矩阵计算

10.1 目的

核心任务：

  • 计算观测模型对误差状态的雅可比矩阵HH
  • 这是EKF更新步骤的关键计算

10.2 链式求导法则

单个标记点的雅可比矩阵： Hi=∂h(x^)∂x^=∂h(x^)∂p^cl∂p^cl∂x^H_i = \frac{\partial h(\hat{x})}{\partial \hat{x}} = \frac{\partial h(\hat{x})}{\partial \hat{p}_c^l} \frac{\partial \hat{p}_c^l}{\partial \hat{x}}

第一部分：观测对相机坐标的导数 ∂hi(x^)∂p^cl=[fx0cx0fycy000][1c0−ac201c−bc2000]\frac{\partial h_i(\hat{x})}{\partial \hat{p}_c^l} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} \frac{1}{c} & 0 & -\frac{a}{c^2} \\ 0 & \frac{1}{c} & -\frac{b}{c^2} \\ 0 & 0 & 0 \end{bmatrix}

其中p^cl=[aT bT cT]T\hat{p}_c^l = [a^T \ b^T \ c^T]^T

10.3 位置导数

对IMU位置的导数： ∂p^cl∂pwi=−(C(qwi)C(qci))T\frac{\partial \hat{p}_c^l}{\partial p_w^i} = -(C(q_w^i)C(q_c^i))^T

对相机位置的导数： ∂p^cl∂pci=−C(qci)T\frac{\partial \hat{p}_c^l}{\partial p_c^i} = -C(q_c^i)^T

10.4 旋转导数（小扰动模型）

关键问题：

  • 旋转变量属于李群，不能直接求导
  • 必须使用小扰动模型

一阶扰动理论： f(exp⁡(δθ∧)R)≈f(R)+J⋅δθf(\exp(\delta\theta^\wedge)R) \approx f(R) + J \cdot \delta\theta

右扰动： ∂R(θ+δθ)⋅a∂δθ≈−R(θ)⋅a∧⋅Jr(θ)\frac{\partial R(\theta + \delta\theta) \cdot a}{\partial \delta\theta} \approx -R(\theta) \cdot a^\wedge \cdot J_r(\theta)

右雅可比： Jr(θ)=I−1−cos⁡∥θ∥∥θ∥2θ∧+∥θ∥−sin⁡∥θ∥∥θ∥3(θ∧)2J_r(\theta) = I - \frac{1-\cos\|\theta\|}{\|\theta\|^2}\theta^\wedge + \frac{\|\theta\|-\sin\|\theta\|}{\|\theta\|^3}(\theta^\wedge)^2

具体导数：

  • ∂pcl∂δθw\frac{\partial p_c^l}{\partial \delta\theta_w}：对IMU姿态的导数
  • ∂pcl∂δθi\frac{\partial p_c^l}{\partial \delta\theta_i}：对相机外参姿态的导数
  • ∂pcl∂δθo\frac{\partial p_c^l}{\partial \delta\theta_o}：对视觉框架姿态的导数

10.5 全局雅可比矩阵

组合所有特征点： 将所有特征点的雅可比矩阵组合成2n×272n \times 27维的全局雅可比矩阵HH，其中nn是标记点数量。

10.6 意义

  • 线性化关键：雅可比矩阵是EKF线性化的核心
  • 计算效率：链式求导法则分解复杂计算
  • 几何正确性：小扰动模型保证旋转导数的几何正确性
  • 实时性：虽然计算复杂，但可以高效实现

十一、EKF更新过程

11.1 完整流程

1. 观测模型线性化：

  • 计算雅可比矩阵HH
  • 使用小扰动模型处理旋转变量

2. 应用EKF更新：

  • 计算卡尔曼增益KK
  • 更新状态估计x^\hat{x}
  • 更新协方差矩阵PP

3. 关键注意事项：

  • 测量误差协方差矩阵RR由测量传感器确定
  • 姿态更新时必须使用小扰动模型
  • 不能直接在欧几里得空间进行简单加法

11.2 更新公式回顾

卡尔曼增益： K=Pk∣k−1HT(HPk∣k−1HT+R)−1K = P_{k|k-1} H^T (H P_{k|k-1} H^T + R)^{-1}

状态更新： x^k∣k=x^k∣k−1+K(z−h(x^k∣k−1))\hat{x}_{k|k} = \hat{x}_{k|k-1} + K(z - h(\hat{x}_{k|k-1}))

协方差更新： Pk∣k=(I−KH)Pk∣k−1P_{k|k} = (I - K H) P_{k|k-1}

11.3 意义

  • 融合完成：成功将视觉和惯性信息融合
  • 实时估计：提供目标与无人机之间的相对位置与姿态
  • 状态支持：为后续轨迹跟踪提供关键的状态估计支持

十二、整体系统总结

12.1 解决的问题

  1. GNSS失效问题：在复杂环境下提供可靠的定位
  2. 传感器融合：有效融合视觉和惯性信息
  3. 实时性要求：满足无人机实时跟踪的需求
  4. 精度要求：提供高精度的位置和姿态估计

12.2 技术亮点

  1. 小扰动模型：正确处理旋转和位姿的非线性特性
  2. 误差状态EKF：保持主状态的几何约束
  3. 联合标定：精确的传感器外参和时间同步
  4. 数值积分：RK4方法保证数值精度

12.3 应用价值

  • 自主导航：为无人机提供可靠的自主定位能力
  • 目标跟踪：支持精确的目标跟踪任务
  • 复杂环境：在GNSS失效环境下仍能工作
  • 实时性能：满足实时系统的计算要求

十三、关键数学概念总结

13.1 坐标系变换

  • 刚体变换：TABT_{AB}表示从B到A的变换
  • 旋转矩阵：C(q)C(q)从四元数qq计算旋转矩阵
  • 坐标变换链：世界→IMU→相机→目标

13.2 李群与李代数

  • SO(3)：三维旋转群
  • SE(3)：三维刚体变换群
  • 指数映射：exp⁡(ξ∧)\exp(\xi^\wedge)将李代数映射到李群
  • 小扰动：δθ\delta\theta表示旋转向量

13.3 状态估计

  • 主状态：使用四元数等非线性表示
  • 误差状态：使用小扰动表示误差
  • 协方差传播：描述状态不确定性的演化

13.4 数值方法

  • RK4积分：高精度数值积分
  • 泰勒展开：近似矩阵指数
  • 链式求导：计算复杂雅可比矩阵

十四、实际实现考虑

14.1 计算效率

  • 稀疏矩阵：利用雅可比矩阵的稀疏性
  • 并行计算：标记点处理可以并行
  • 优化库：使用高效的线性代数库

14.2 数值稳定性

  • 四元数归一化：每次更新后归一化
  • 协方差正定性：确保协方差矩阵正定
  • 数值阈值：避免除零等数值问题

14.3 鲁棒性

  • 异常检测：检测和剔除异常观测
  • 初始化：良好的初始状态估计
  • 故障处理：传感器失效时的处理策略

结语

基于扩展卡尔曼滤波的视觉惯性融合定位方案是一个完整的、理论严谨的、工程实用的定位系统。它通过巧妙的数学工具（小扰动模型、误差状态EKF）解决了非线性系统状态估计的难题，通过传感器融合（视觉+惯性）解决了GNSS失效环境下的定位问题，为无人机的自主跟踪任务提供了可靠的状态估计支持。

本文作者:cc

本文链接:

版权声明:本博客所有文章除特别声明外，均采用 BY-NC-SA 许可协议。转载请注明出处！