基于平面运动的ESKF多传感器融合

初始位姿的获取

静止时，测量 imu 的重力加速度和零偏，根据重力加速度计算 roll 和 pitch
机器初始化阶段会回到 x、y、z 为 0 的位置，所以一开始默认位置为 0
根据视觉测量的 yaw 角，确认开始时的 yaw 值

约束条件：

直线约束：机器只在 x 轴上做直线运动，因此 y 和 z 运动过程应该为 0，pitch、roll 值应该为 0
视觉约束：视觉摄像头一直能观测到 y 和 yaw 值，观测到标志物时，可以同时确认 x 值（循线摄像头和 imu 外参不标定，直接使用其观测值，有一定误差）
静止约束：机器运动过程存在静止状态，可以实时计算零偏、重力加速度和零速修正（ZUPT）
odom 约束：encoder 会观测 x 速度

问题点

可先运行几秒，用互相关粗略估计各传感器时间相对延迟
时间戳对齐 + 插值/外推 + 滑动窗口优化
静止时更新和相机这这两可能会丢失一个

误差状态卡尔曼滤波

状态定义

名义状态：x=[pˉvˉRˉbaˉbgˉaˉ]\mathbf{x} = \begin{bmatrix} \bar p &\bar v &\bar R &\bar {ba} &\bar {bg} &\bar a \end{bmatrix}x=[pˉvˉRˉbaˉbgˉaˉ]

这六个变量均为 imu 在世界坐标系下的值，其中 p 为平移，v 为速度，R 为姿态，bab_aba为角速度零偏，bgb_gbg为线加速度零偏
a：平面的坡度
imu 存在比例因子需要标定问题（BMI088 型号大概存在 1%的误差），9.8 的重力加速度会测量为 9.74。暂时不考虑重力加速度模在全球范围的不同，看后续发展在考虑是否加入优化

误差状态：δx=[δpδvδθδbaδbgδa]\mathbf{\delta x} = \begin{bmatrix}\delta p & \delta v & \delta \theta & \delta ba & \delta bg & \delta a \end{bmatrix}δx=[δpδvδθδbaδbgδa]

名义状态预测（IMU 积分）：

IMU 在离散时间下的递推公式：

p(t+Δt)=p(t)+vΔt+12(R(a~−ba))Δt2+12gΔt2,v(t+Δt)=v(t)+R(a~−ba)Δt+gΔt,R(t+Δt)=R(t)Exp((ω~−bg)Δt),bg(t+Δt)=bg(t),ba(t+Δt)=ba(t).(1) \begin{align*} p(t + \Delta t) &= p(t) + v\Delta t + \frac{1}{2}(R(\tilde{a} - b_a))\Delta t^2 + \frac{1}{2}g\Delta t^2, \\ v(t + \Delta t) &= v(t) + R(\tilde{a} - b_a)\Delta t + g\Delta t, \\ R(t + \Delta t) &= R(t)Exp((\tilde{\omega} - b_g)\Delta t), \\ b_g(t + \Delta t) &= b_g(t), \\ b_a(t + \Delta t) &= b_a(t). \end{align*} \tag{1} p(t+Δt)v(t+Δt)R(t+Δt)bg(t+Δt)ba(t+Δt)=p(t)+vΔt+21(R(a~−ba))Δt2+21gΔt2,=v(t)+R(a~−ba)Δt+gΔt,=R(t)Exp((ω~−bg)Δt),=bg(t),=ba(t).(1)

误差状态预测

通过忽略状态量的二阶小量，可推出误差状态的线性的递推公式：

δp(t+Δt)=δp+δvΔt,δv(t+Δt)=δv+(−R(a~−ba)∧δθ−Rδba+δg)Δt−ηv,δθ(t+Δt)=Exp(−(ω−bg)Δt)δθ−δbgΔt−ηθ,δba(t+Δt)=δba+ηa,δbg(t+Δt)=δbg+ηg.(2) \begin{align*} \delta p(t + \Delta t) &= \delta p + \delta v\Delta t, \\ \delta v(t + \Delta t) &= \delta v + (-R(\tilde{a} - b_a)^\wedge\delta\theta - R\delta b_a + \delta g)\Delta t - \eta_v, \\ \delta\theta(t + \Delta t) &= Exp(-(\omega - b_g)\Delta t)\delta\theta - \delta b_g\Delta t - \eta_\theta, \\ \delta b_a(t + \Delta t) &= \delta b_a + \eta_a,\\ \delta b_g(t + \Delta t) &= \delta b_g + \eta_g. \end{align*} \tag{2} δp(t+Δt)δv(t+Δt)δθ(t+Δt)δba(t+Δt)δbg(t+Δt)=δp+δvΔt,=δv+(−R(a~−ba)∧δθ−Rδba+δg)Δt−ηv,=Exp(−(ω−bg)Δt)δθ−δbgΔt−ηθ,=δba+ηa,=δbg+ηg.(2)

ηv\eta_vηv 为 ηa∗Δt\eta_a* \Delta tηa∗Δt，ηθ\eta_\thetaηθ 为ηg∗Δt\eta_g * \Delta tηg∗Δt，其中ηa\eta_aηa为线加速度，ηg\eta_gηg为角速度

ηg、ηa\eta_g 、 \eta_aηg、ηa 为高斯过程，其均值为0，协方差函数为Cov(bgb_gbg)δ(t−t′)、 Cov(bab_aba)δ(t−t′)其中，δ 函数是一个单位脉冲函数，表示只有当时间 t 和 t′相同时，两个变量才有相关性。

根据公式（2）可得到 ESKF 的转移矩阵 F 及转换矩阵噪声的协方差 Q：

F=[IIΔt00000I−(R(a~−ba)∧)Δt−RΔt0000Exp(−(ω~−bg)Δt)0−IΔt0000I000000I0000001].(3) F = \begin{bmatrix} I & I\Delta t & 0 & 0 & 0 & 0\\ 0 & I & -(R(\tilde{a} - b_a)^\wedge)\Delta t & -R\Delta t & 0& 0 \\ 0 & 0 & Exp(-(\tilde{ω} - b_g)\Delta t) & 0& -I\Delta t & 0 \\ 0 & 0 & 0 & I & 0 & 0 \\ 0 & 0 & 0 & 0 & I & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix}. \tag{3} F= I00000IΔtI00000−(R(a~−ba)∧)ΔtExp(−(ω~−bg)Δt)0000−RΔt0I0000−IΔt0I0000001 .(3)

Q=diag(03,Cov(ηv),Cov(ηθ),Cov(ηa),Cov(ηg),1e−8∼1e−4)(4) Q = diag(0_3, Cov(\eta_v), Cov(\eta_\theta), Cov(\eta_a), Cov(\eta_g),1e-8 \sim 1e-4) \tag{4} Q=diag(03,Cov(ηv),Cov(ηθ),Cov(ηa),Cov(ηg),1e−8∼1e−4)(4)

第一项的位置过程没有噪声，所以为 0。协方差第二项和第三项使用 imu 静止时测量值的方差，第四和第五项是零偏变化的协方差，可根据 Allan 方差中，零偏随机游走系数计算，一般设置较小，如 Cov(ηg)=diag([1e−12,1e−12,1e−12])Cov(\eta_g) = diag([1e-12, 1e-12, 1e-12])Cov(ηg)=diag([1e−12,1e−12,1e−12]) (单位：(rad/s)²/Hz)；Cov(ηa)=diag([1e−8,1e−8,1e−8])Cov(\eta_a) = diag([1e-8, 1e-8, 1e-8])Cov(ηa)=diag([1e−8,1e−8,1e−8]) (单位：(m/s³)²/Hz)

误差预测：

δxpred=Fδx,Ppred=FPF⊤+Q.(5) \delta x_{pred} = F\delta x, \\ P_{pred} = FPF^\top + Q.\tag{5} δxpred=Fδx,Ppred=FPF⊤+Q.(5)

带小斜坡的平面约束（z）

两个约束方式可选，这里选择第二种：

（1）零垂向速度软约束（适用于低速场景）：

vz≈0 v_z \approx 0 vz≈0

作为观测：

zobs=0=vz+v,σv=0.05∼0.2 m/s z_{\text{obs}} = 0 = v_z + v, \quad \sigma_v = 0.05 \sim 0.2~\text{m/s} zobs=0=vz+v,σv=0.05∼0.2 m/s

注意：若坡度为 α\alphaα，真实 vz=vhoriz⋅sin⁡α≈vhoriz⋅αv_z = v_{\text{horiz}} \cdot \sin\alpha \approx v_{\text{horiz}} \cdot \alphavz=vhoriz⋅sinα≈vhoriz⋅α

例如：速度 1 m/s，坡度 2°（≈0.035 rad）→ ( v_z ≈ 0.035 ) m/s

所以 σv\sigma_vσv 应 ≥ 0.05 才不会压制真实坡度。

（2）估计平面参数：运动应位于平面 z=ax+by+cz = a x + b y + cz=ax+by+c 作为一个软约束

可以假设机器在直线上运动，且以 imu 与地面的距离为 c，因此 y 轴不考虑，c 默认是 z 的高度值。

约束模型：z=ax+cz = a x + cz=ax+c

观测值：zobs=0z_{\text{obs}} = 0zobs=0

观测函数：

hplane(x)=z−(ax+c) h_{\text{plane}}(\mathbf{x}) = z - (a x + c) hplane(x)=z−(ax+c)

zobs=hplane(x)+v,v∼N(0,σplane2)z_{\text{obs}} = h_{\text{plane}}(\mathbf{x})+ v, \quad v \sim \mathcal{N}(0, \sigma_{\text{plane}}^2)zobs=hplane(x)+v,v∼N(0,σplane2)

代入名义状态和误差状态：

hplane(x)=zˉ+δz−((aˉ+δa)(xˉ+δx)+c) h_{\text{plane}}(\mathbf{x}) = \bar z + \delta z - ((\bar a + \delta a) (\bar x + \delta x) + c) hplane(x)=zˉ+δz−((aˉ+δa)(xˉ+δx)+c)

所以，该观测函数对误差状态的雅可比矩阵为：

∂h∂δx=[−a0101×12−x] \frac{\partial h}{\partial \mathbf{\delta x}} = \begin{bmatrix} -a & 0 & 1 & 0_{1\times12} & -x & \end{bmatrix} ∂δx∂h=[−a0101×12−x]

协方差设置：

σplane\sigma_{\text{plane}}σplane 控制约束强度：

高精度平整路面（如室内地板） 0.01 -- 0.02 m

户外沥青/水泥路（轻微起伏） 0.05 -- 0.1 m

草地、土路、有减速带 0.1 -- 0.2 m

aaa 的过程噪声

情况 1：全局单一平面（如整个任务在一个斜坡上）
将 aaa 视为常值参数
过程噪声 Qa=0Q_{a} = 0Qa=0（或极小）
情况 2：分段平面（如经过多个不同坡度区域）
给 aaa 添加小的过程噪声：
a˙=wa,w∼N(0,qslope) \dot{a} = w_a,\quad w \sim \mathcal{N}(0, q_{\text{slope}}) a˙=wa,w∼N(0,qslope)
qslopeq_{\text{slope}}qslope 可设为 (10−5∼10−3)2(10^{-5} \sim 10^{-3})^2(10−5∼10−3)2，允许缓慢变化
情况 3：结合静止检测优化平面
静止时，已用重力方向约束 imu 位姿了，是否还需要用来作为额外观测更新 aaa

运动约束：pitch、roll 不变，即世界坐标系下的角速度 x、y 为 0

世界坐标系下的角速度在 x 和 y 方向为零：

ωxw=0,ωyw=0 \omega^w_x = 0,\quad \omega^w_y = 0 ωxw=0,ωyw=0

其中世界系角速度由 IMU 测量和当前状态计算：

ωw=R(ωb−bg) \boldsymbol{\omega}^w = \mathbf{R} (\boldsymbol{\omega}^b - \mathbf{b}_g) ωw=R(ωb−bg)

R∈SO(3)\mathbf{R} \in \mathrm{SO}(3)R∈SO(3)：当前估计的旋转矩阵（从机体到世界）
ωb\boldsymbol{\omega}^bωb：陀螺仪原始测量
bg\mathbf{b}_gbg：陀螺仪零偏（bias）

右扰动模型

真实旋转 = 估计旋转 × 小扰动

Rtrue=R⋅exp⁡([δθ]×) \mathbf{R}{\text{true}} = \mathbf{R} \cdot \exp([\boldsymbol{\delta\theta}]\times) Rtrue=R⋅exp([δθ]×)

为什么使用右扰动？

可以看到右扰动是在右边乘一个微小的旋转向量，这与我们运动模式一致，当有下一次旋转时（imu 重新测量了新值时），旋转量就是在右边相乘。

观测函数与目标

定义观测函数：

h(R,bg)=(R(ωb−bg))[:2,:]=[ωxwωyw] h(\mathbf{R}, \mathbf{b}_g) = \left( \mathbf{R} (\boldsymbol{\omega}^b - \mathbf{b}_g) \right)[:2, :] = \begin{bmatrix} \omega^w_x \\ \omega^w_y \end{bmatrix} h(R,bg)=(R(ωb−bg))[:2,:]=[ωxwωyw]

虚拟观测值为：

z=[00] \mathbf{z} = \begin{bmatrix} 0 \\ 0 \end{bmatrix} z=[00]

我们需要计算观测函数对误差状态 δx\delta \mathbf{x}δx 的雅可比矩阵：

H=∂h∂δx=[00∂h∂δθ0∂h∂δbg0]∈R2×16 \mathbf{H} = \frac{\partial h}{\partial \delta \mathbf{x}} = \begin{bmatrix} \boldsymbol 0 & \boldsymbol 0 & \displaystyle \frac{\partial h}{\partial \boldsymbol{\delta\theta}} & \boldsymbol 0 & \displaystyle \frac{\partial h}{\partial \boldsymbol{\delta b}_g} & 0 \end{bmatrix} \in \mathbb{R}^{2 \times 16} H=∂δx∂h=[00∂δθ∂h0∂δbg∂h0]∈R2×16

对δbg\delta {\mathbf{b}_g}δbg的导数

代入名义状态和误差状态：

h(R,bg)=(Rˉ(ωbˉ−bgˉ−δbg))[:2,:] h(\mathbf{R}, \mathbf{b}_g) = \left( \mathbf{\bar R} (\bar {\boldsymbol{\omega}^b} - \bar {\mathbf{b}_g} - \delta {\mathbf{b}_g}) \right)[:2, :] h(R,bg)=(Rˉ(ωbˉ−bgˉ−δbg))[:2,:]

所以，

∂h∂δbg=−R[:2,:] \displaystyle \frac{\partial h}{\partial \boldsymbol{\delta b}_g} = -\mathbf{R}[:2, :] ∂δbg∂h=−R[:2,:]

对 δθ\mathbf {\delta\theta}δθ 导数

使用右扰动模型代入名义状态和误差状态：

h(R,bg)=(Rˉexp⁡([δθ]×)(ωbˉ−bgˉ))[:2,:] h(\mathbf{R}, \mathbf{b}g) = \left( \mathbf{\bar R} \exp([\boldsymbol{\delta\theta}]\times) (\bar {\boldsymbol{\omega}^b} - \bar {\mathbf{b}_g}) \right)[:2, :] h(R,bg)=(Rˉexp([δθ]×)(ωbˉ−bgˉ))[:2,:]

为方便书写，暂时忽略 [:2,:][:2, :][:2,:]，且设

v:=ωb−bg(去偏后的机体角速度) \mathbf{v} := \boldsymbol{\omega}^b - \mathbf{b}_g \quad (\text{去偏后的机体角速度}) v:=ωb−bg(去偏后的机体角速度)

并且已知 δθ ≪1\boldsymbol{\delta\theta}\ \ll 1δθ ≪1：

exp⁡([δθ]×)≈I+[δθ]× \exp([\boldsymbol{\delta\theta}]\times) \approx \mathbf{I} + [\boldsymbol{\delta\theta}]\times exp([δθ]×)≈I+[δθ]×

且有叉积恒等式

δθ\]×v=δθ×v=−v×δθ=−\[v\]×δθ \[\\boldsymbol{\\delta\\theta}\]_\\times \\mathbf{v} = \\boldsymbol{\\delta\\theta} \\times \\mathbf{v} = - \\mathbf{v} \\times \\boldsymbol{\\delta\\theta} = - \[\\mathbf{v}\]_\\times \\boldsymbol{\\delta\\theta} \[δθ\]×v=δθ×v=−v×δθ=−\[v\]×δθ 则： ∂h=(Rˉexp⁡(\[δθ\]×))v−(Rˉ)v≈Rˉ(I+\[δθ\]×)v−(Rˉ)v=Rˉ\[δθ\]×v=−Rˉ\[v\]×δθ \\begin{aligned} \\partial h \&= \\left( \\mathbf{\\bar R} \\exp(\[\\boldsymbol{\\delta\\theta}\]_\\times) \\right) \\mathbf{v} - \\left( \\mathbf{\\bar R} \\right) \\mathbf{v} \\\\ \&\\approx \\mathbf{\\bar R}(\\mathbf{I} + \[\\boldsymbol{\\delta\\theta}\]_\\times)\\mathbf{v} - \\left( \\mathbf{\\bar R} \\right) \\mathbf{v} \\\\ \&= \\mathbf{\\bar R} \[\\boldsymbol{\\delta\\theta}\]_\\times \\mathbf{v} \\\\ \&= - \\bar {\\mathbf{R}} \[\\mathbf{v}\]_\\times \\boldsymbol{\\delta\\theta} \\end{aligned} ∂h=(Rˉexp(\[δθ\]×))v−(Rˉ)v≈Rˉ(I+\[δθ\]×)v−(Rˉ)v=Rˉ\[δθ\]×v=−Rˉ\[v\]×δθ 所以，对 δθ\\delta \\thetaδθ 的导数为： ∂h∂δθ=(−Rˉ\[v\]×)\[:2,:\] \\frac{\\partial h}{\\partial \\boldsymbol{\\delta\\theta}} = \\left( - \\bar {\\mathbf{R}} \[\\mathbf{v}\]_\\times \\right)\[:2, :\] ∂δθ∂h=(−Rˉ\[v\]×)\[:2,:

观测协方差设置

需要考虑陀螺仪测量噪声和运动过程因平面不平或者控制问题导致的误差，即：

Robs=(σg2+σsys2) I2 \boxed{ \mathbf{R}{\text{obs}} = (\sigma_g^2 + \sigma{\text{sys}}^2) \, \mathbf{I}_2 } Robs=(σg2+σsys2)I2

σg\sigma_gσg 描述陀螺仪测量噪声，一般取可以标定或者查官方文档。如 MPU6050: σg≈0.01 rad/s\sigma_g \approx 0.01~\text{rad/s}σg≈0.01 rad/s → Robs=(0.01)2I2=1 ⁣× ⁣10−4 I2\mathbf{R}_{\text{obs}} = (0.01)^2 \mathbf{I}_2 = 1\!\times\!10^{-4} \, \mathbf{I}2Robs=(0.01)2I2=1×10−4I2
σsys\sigma{\text{sys}}σsys 表示你对"平面有多平"的信心

若地面较平整（如室内地板），可设 σsys≈0.01∼0.05 rad/s\sigma_{\text{sys}} \approx 0.01 \sim 0.05~\text{rad/s}σsys≈0.01∼0.05 rad/s

若地面粗糙（如户外草地），可设 σsys≈0.1 rad/s\sigma_{\text{sys}} \approx 0.1~\text{rad/s}σsys≈0.1 rad/s

静止约束（重力加速度、零偏）

静止时，可测量平均值为重力加速度的方向，模取值为 9.80665 m/s^2，作差之后是零偏。这里测量了重力加速度，因此也可以约束重力加速度的模，但是此处暂时不考虑该影响因素。

世界坐标系下的重力加速度：gw=[00−9.80665]g^w = \begin{bmatrix} 0 \\ 0 \\ -9.80665 \end{bmatrix}gw= 00−9.80665 ，
g=9.80665m/s2g = 9.80665 m/s^2g=9.80665m/s2

观测量：imu 坐标系下的重力加速度gig^igi、零偏bg、bab_g、b_abg、ba

零偏的观测函数为：

h(ba,bg)=[babg] h(\mathbf{b}_a, \mathbf{b}_g) = \begin{bmatrix} \mathbf{b}_a \\ \mathbf{b}_g \end{bmatrix} h(ba,bg)=[babg]

重力加速度的观测函数：

h(R,ba)=RTgw+ba h(\mathbf{R}, \mathbf{b}_a) = \mathbf{R}^T \mathbf{g}^w + \mathbf{b}_a h(R,ba)=RTgw+ba

零偏观测矩阵

易得：

H=∂h∂δx=[000110]∈R2×16 \mathbf{H} = \frac{\partial h}{\partial \delta \mathbf{x}} = \begin{bmatrix} \boldsymbol 0 & \boldsymbol 0 & \boldsymbol 0 & \boldsymbol 1 & \boldsymbol 1 & 0 \end{bmatrix} \in \mathbb{R}^{2 \times 16} H=∂δx∂h=[000110]∈R2×16

重力加速度观测函数

我们需要计算观测函数对误差状态 δx\delta \mathbf{x}δx 的雅可比矩阵：

H=∂h∂δx=[00∂h∂δθ∂h∂δba00]∈R2×16 \mathbf{H} = \frac{\partial h}{\partial \delta \mathbf{x}} = \begin{bmatrix} \boldsymbol 0 & \boldsymbol 0 & \displaystyle \frac{\partial h}{\partial \boldsymbol{\delta\theta}} & \displaystyle \frac{\partial h}{\partial \boldsymbol{\delta b}_a} & \boldsymbol 0 & 0 \end{bmatrix} \in \mathbb{R}^{2 \times 16} H=∂δx∂h=[00∂δθ∂h∂δba∂h00]∈R2×16

对δbg\delta {\mathbf{b}_g}δbg的导数

易得：

∂h∂δba=I \displaystyle \frac{\partial h}{\partial \boldsymbol{\delta b}_a} = I ∂δba∂h=I

对δθ\delta \thetaδθ的导数

误差模型为：

Rtrue=Rexp⁡([δθ]×)≈R(I+[δθ]×) \mathbf{R}{true} = \mathbf{R} \exp([\delta \theta]\times) \approx \mathbf{R} (\mathbf{I} + [\delta \theta]_\times) Rtrue=Rexp([δθ]×)≈R(I+[δθ]×)

⇒RtrueT≈(I−[δθ]×)RT \Rightarrow \mathbf{R}{true}^T \approx (\mathbf{I} - [\delta \theta]\times) \mathbf{R}^T ⇒RtrueT≈(I−[δθ]×)RT

将其带入重力加速度的观测函数：

∂h≈(I−[δθ]×)RTgw−RTgw=(−[δθ]×)RTgw=(RTgw)×δθ \begin{aligned} \partial h & \approx (\mathbf{I} - [\delta \theta]\times) \mathbf{R}^Tg^w - \mathbf{R}^Tg^w \\ &= (-[\delta \theta]\times) \mathbf{R}^Tg^w \\ &= (\mathbf{R}^T \mathbf{g}^w)_\times \delta \theta \end{aligned} ∂h≈(I−[δθ]×)RTgw−RTgw=(−[δθ]×)RTgw=(RTgw)×δθ

取 g=9.80665m/s2g = 9.80665 m/s^2g=9.80665m/s2，对 δθ\delta \thetaδθ 的导数为：

∂h∂δθ=(RTgw)×=[0gR33−gR32gR330gR31gR32−gR310] \begin{aligned} \frac{\partial h}{\partial \boldsymbol{\delta\theta}} &= (\mathbf{R}^T \mathbf{g}^w)\times \\ &= \begin{bmatrix} 0 & g R{33} & -g R_{32} \\ g R_{33} & 0 & g R_{31} \\ g R_{32} & -g R_{31} & 0 \end{bmatrix} \end{aligned} ∂δθ∂h=(RTgw)×= 0gR33gR32gR330−gR31−gR32gR310

因为静止时，加速度计测量满足：
∥a−b_a∥≈g \|\mathbf{a} - \mathbf{b}\_a\| \approx g ∥a−b_a∥≈g

即三个分量受一个非线性约束 （位于球面上），所以只有两个自由度,第三个分量只相差一个正负号，但是名义状态又提供了先验方向。用全部三个分量会导致：

观测矩阵秩亏（rank-deficient）；
卡尔曼增益计算不稳定（HPHT+R\mathbf{H} \mathbf{P} \mathbf{H}^T + \mathbf{R}HPHT+R 接近奇异）。
因此，必须只选两个线性无关的分量。

因此，观测矩阵的约束只能选择两行，对此处，选择 imu 坐标下的重力加速度在各个轴的投影分量最小的两个轴，因为重力在某轴上越大，该轴对姿态扰动越不敏感（导数小），约束效果就不好。

观测协方差设置

噪声来源：

加速度计本征噪声（白噪声）；
静止假设不完美（微振动、平台抖动）；
数值误差、量化误差。
直接使用静止状态下的方差作为协方差矩阵。

零速修正（ZUPT）

观测量：imu 坐标系、世界坐标系下速度和角速度为 0，角速度为 0，可以参考运动约束。

这里使用速度为 0 作为观测函数，其雅可比矩阵，易得：

H=∂h∂δx=[0I0000]∈R2×16 \mathbf{H} = \frac{\partial h}{\partial \delta \mathbf{x}} = \begin{bmatrix} \boldsymbol 0 & \boldsymbol I & \boldsymbol 0 & \boldsymbol 0 & \boldsymbol 0 & 0 \end{bmatrix} \in \mathbb{R}^{2 \times 16} H=∂δx∂h=[0I0000]∈R2×16

协方差使用上一次静态观测的协方差矩阵。

轮式编码器约束

观测量：

世界坐标系下的速度：vw=[vxvyvz]=[vx00]v^w = \begin{bmatrix} v_x \\ v_y \\ v_z \end{bmatrix} = \begin{bmatrix} v_x \\ 0 \\ 0 \end{bmatrix}vw= vxvyvz = vx00

imu 坐标系下的速度：vi=RTvwv^i = R^Tv^wvi=RTvw

使用vwv^wvw，观测函数雅可比矩阵，易得：

观测协方差设置

噪声来源：

轮式编码器本征噪声（白噪声）；
vxv_xvx，是我们真正测量的，vyv_yvy 和 vzv_zvz 是虚拟观测，侧滑和地面不平时则不是 0 了。vyv_yvy 和 vzv_zvz协方差可以给大一些，这种方式既尊重物理约束，又保留了对异常（如侧滑）的鲁棒性。

相机观测约束

观测量：世界坐标系下的 x、y、yaw 角，x 和 y 轴的观测推导很简单，接下来只关注 yaw 角。必须考虑的一点时，循线摄像头跟双目相机之间并没有标定，这里面肯定存在系统误差，后续可以考虑添加新的状态量在线标定这些误差。

循线的 yaw 角跟双目相机 yaw 角默认相差 90 度，即yawstereo=yawfollow+π/2yaw_{stereo} = yaw_{follow} + \pi/2yawstereo=yawfollow+π/2
以双目相机的 z 轴为基准vc=[001]Tv_c = \begin{bmatrix} 0 & 0 & 1 \end{bmatrix}^Tvc=[001]T，计算其在 imu 坐标系下的坐标：vi=Ricvcv^i = R_{ic}v^cvi=Ricvc
再通过名义状态的旋转矩阵，计算出世界坐标系下的 yaw 角：vw=Rwivi=Rviv^w = R_{wi}v^i = Rv^ivw=Rwivi=Rvi
观测函数：
h(δθ)=atan2⁡(vyw,vxw) h(\boldsymbol{\delta \theta}) = \operatorname{atan2}\big( v^w_y,v^w_x \big) h(δθ)=atan2(vyw,vxw)

yaw 角观测函数的雅可比矩阵：

对于任意 (x,y)≠(0,0)(x, y) \ne (0, 0)(x,y)=(0,0)，函数 θ=atan2⁡(y,x)\theta = \operatorname{atan2}(y, x)θ=atan2(y,x)的全微分为：

d(atan2⁡(y,x))=−y dx+x dyx2+y2 d\big(\operatorname{atan2}(y, x)\big) = \frac{-y\, dx + x\, dy}{x^2 + y^2} d(atan2(y,x))=x2+y2−ydx+xdy

所以：

dh=−vyw dvxw+vxw dvyw(vxw)2+(vyw)2 dh = \frac{-v^w_y\, dv^w_x + v^w_x\, dv^w_y}{(v^w_x)^2 + (v^w_y)^2} dh=(vxw)2+(vyw)2−vywdvxw+vxwdvyw

所以：

dhdδθ=−vyw dvxwdδθ+vxw ddvywdδθ(vxw)2+(vyw)2 \frac{dh}{d\delta \theta} = \frac{-v^w_y\, \frac{dv^w_x}{d\delta \theta}+ v^w_x\, d\frac{dv^w_y}{d\delta \theta}}{(v^w_x)^2 + (v^w_y)^2} dδθdh=(vxw)2+(vyw)2−vywdδθdvxw+vxwddδθdvyw

因为vw=Rwiviv^w = R_{wi}v^ivw=Rwivi，所以：

∂vw=R⋅exp⁡([δθ]×)vi−Rvi=R⋅(I+[δθ]×)vi−Rvi=R⋅[δθ]×vi=−R[vi]×δθ \begin{aligned} \partial v^w &= \mathbf{R} \cdot \exp([\boldsymbol{\delta\theta}]\times)v^i - \mathbf{R}v^i \\ &= \mathbf{R} \cdot (I + [\delta \theta]\times)v^i- \mathbf{R}v^i \\ &= \mathbf{R} \cdot [\delta \theta]\times v^i \\ &= -\mathbf{R}[v^i]\times \delta \theta \end{aligned} ∂vw=R⋅exp([δθ]×)vi−Rvi=R⋅(I+[δθ]×)vi−Rvi=R⋅[δθ]×vi=−R[vi]×δθ

所以：

∂vw∂δθ=−R[vi]× \frac{\partial v^w}{\partial \delta \theta} = -\mathbf{R}[v^i]_\times ∂δθ∂vw=−R[vi]×

将其代入，即可求得dhdδθ\frac{dh}{d\delta \theta}dδθdh。