 ESKF模型

𝑞
状态量： 𝑥 =
𝑤𝑏
Ture-state： nominal-state：
· 1 · 1
𝑞𝑡 = 𝑞𝑡 ⊗ 𝑤𝑚 − 𝑤𝑏𝑡 − 𝑤𝑛 𝑞 = 𝑞 ⊗ 𝑤𝑚 − 𝑤𝑏
2 140 2 142
· ·
𝑤𝑏𝑡 = 𝑤𝑏𝑛 𝑤𝑏 = 0
1
取： 𝑞𝑡 = 𝑞 ⊗ 𝛿𝑞 𝑤𝑏𝑡 = 𝑤𝑏 + 𝛿𝑤𝑏
𝛿𝑞 ≈ 𝑇𝑎𝑏𝑙𝑒2
𝛿 𝜃 Τ2
得到误差状态error-state的运动模型微分形式：
·
𝛿𝜃 = − 𝑤𝑚 − 𝑤𝑏 × ⋅ 𝛿𝜃 − 𝛿𝑤𝑏 − 𝑤𝑛
· 143
𝛿𝑤𝑏 = 𝑤𝑏𝑛

得到误差状态的运动模型递推表达式：
𝑇
𝛿𝜃 ← 𝑅ሼ 𝑤𝑚 − 𝑤𝑏 𝛥𝑡ሽ ⋅ 𝛿𝜃 − 𝛿𝑤𝑏 ⋅ 𝛥𝑡 − 𝜃𝑖
166
𝛿𝑤𝑏 ← 𝛿𝑤𝑏 + 𝑤𝑖
其中:
𝑤𝑚 − 𝑤𝑏 𝛥𝑡 ≜ 𝑢𝛥𝜃
𝑇
𝑅ሼ 𝑤𝑚 − 𝑤𝑏 ሽ = 𝑅ሼ−𝑢𝛥𝜃ሽ = 𝐼 − 𝑢 × sin𝛥𝜃 + 𝑢 2× 1 − cos𝛥𝜃
其中 𝜃𝑖 𝑤𝑖 为高斯随机脉冲噪声，均值为0，协方差为 𝑤𝑛 𝑤𝑏𝑛 在 𝛥𝑡 时间内的积分值

𝛩𝑖 = 𝜎𝑤2 𝑛 𝛥𝑡 2 𝐼
𝛺𝑖 = 𝜎𝑤2 𝑏𝑛 𝛥𝑡 2 𝐼
1
  ESKF预测方程

∧ ∧
𝛿𝑥 ← 𝐹𝑥 ⋅ 𝛿𝑥
𝑃 ← 𝐹𝑥 𝑃𝐹𝑥𝑇 + 𝐹𝑖 𝑄𝑖 𝐹𝑥𝑇

对状态变量求偏导：
𝑇
𝐹𝑥 = 𝑅ሼ 𝑤𝑚 − 𝑤𝑏 ⋅ 𝛥𝑡ሽ −𝐼 ⋅ 𝛥𝑡
0 𝐼

对噪声变量求偏导：

𝐼 0 𝛩𝑖 0
𝐹𝑖 = 𝑄𝑖 =
0 𝐼 0 𝛺𝑖

 ESKF观测方程 2
𝑞𝑤 + 𝑞𝑥2 − 𝑞𝑦2 − 𝑞𝑧2 2 𝑞𝑥 𝑞𝑦 − 𝑞𝑤 𝑞𝑧 2 𝑞𝑥 𝑞𝑧 + 𝑞𝑤 𝑞𝑦
2
𝑅= 2 𝑞𝑥 𝑞𝑦 + 𝑞𝑤 𝑞𝑧 𝑞𝑤 − 𝑞𝑥2 + 𝑞𝑦2 − 𝑞𝑧2 2 𝑞𝑦 𝑞𝑧 − 𝑞𝑤 𝑞𝑥
𝑦 = ℎ 𝑥𝑡 + 𝑣 𝑣 ∼ 𝛮ሼ0, 𝑉 ሽ
2
2 𝑞𝑥 𝑞𝑧 − 𝑞𝑤 𝑞𝑦 2 𝑞𝑦 𝑞𝑧 + 𝑞𝑤 𝑞𝑥 𝑞𝑤 − 𝑞𝑥2 − 𝑞𝑦2 + 𝑞𝑧2

𝐾 = 𝑃𝐻𝑇 𝐻𝑃𝐻𝑇 + 𝑉 −1
将加速度计的三个测量作为观测变量：
∧ ∧
𝛿𝑥 ← 𝐾 𝑦 − ℎ 𝑥𝑡 0
𝑇
𝑎𝑚 =𝑅 ⋅ 0
𝑃 ← 𝑃 − 𝐾 𝐻𝑃𝐻𝑇 + 𝑉 𝐾 𝑇 1
𝑎𝑥 2(𝑞𝑥 𝑞𝑧 − 𝑞𝑤 𝑞𝑦 ൯
𝑎𝑦 = 2(𝑞𝑦 𝑞𝑧 + 𝑞𝑤 𝑞𝑥 ൯ + 𝑣
𝑎𝑧 2
𝑞𝑤 − 𝑞𝑥2 − 𝑞𝑦2 + 𝑞𝑧2
2
  ESKF观测方程

𝜕ℎ 𝜕ℎ 𝜕𝑥𝑡
𝐻≜ |𝑥 = | | = 𝐻𝑥 𝑋𝛿𝑥
𝜕𝛿𝑥 𝜕𝑥𝑡 𝑥 𝜕𝛿𝑥 𝑥

其中:

−2𝑞𝑦 2𝑞𝑧 −2𝑞𝑤 2𝑞𝑥 0 0 0

𝐻𝑥 = 2𝑞𝑥 2𝑞𝑤 2𝑞𝑧 2𝑞𝑦 0 0 0
2𝑞𝑤 −2𝑞𝑥 −2𝑞𝑦 2𝑞𝑧 0 0 0

𝜕𝑥𝑡 𝑄 0
𝑋𝛿𝑥 ≜ |𝑥 = 𝛿𝜃
𝜕𝛿𝑥 0 𝐼3

其中: 1
𝜕 1 0 0 0
𝜕 𝑞 ⊗ 𝛿𝑞 𝜕 𝑞 ⊗ 𝛿𝑞 𝜕𝛿𝑞 𝜕 𝑞 𝐿 𝛿𝑞 𝛿𝜃 1 1 0 0
𝑄𝛿𝜃 ≜ |𝑞 = |𝑞 |∧ = |𝑞 2 |∧ = 𝑞𝐿
𝜕𝛿𝜃 𝜕𝛿𝑞 𝜕𝛿𝜃 𝛿𝜃=0 𝜕𝛿𝑞 𝜕𝛿𝜃 𝛿𝜃=0 2 0 1 0
0 0 1
−𝑞𝑥 −𝑞𝑦 −𝑞𝑧
1 𝑞𝑤 −𝑞𝑧 𝑞𝑦
𝑄𝛿𝜃 =
2 𝑞𝑧 𝑞𝑤 −𝑞𝑥
−𝑞𝑦 𝑞𝑥 𝑞𝑤

3
  添加error -state状态到nominal-state
∧
∧ 𝑞 ← 𝑞 ⊗ 𝑞ሼ𝛿𝜃 ሽ
𝑥 ← 𝑥 ⊕ 𝛿𝑥 ∧
𝑤𝑏 ← 𝑤𝑏 + 𝛿𝑤𝑏

 复位error -state
∧
𝛿𝑥 ← 𝑔 𝛿𝑥 = 𝛿𝑥 ⊖ 𝛿𝑥
∧
𝛿𝑥 ← 0
𝑃 ← 𝐺𝑃𝐺 𝑇
1 ∧
𝜕𝑔 𝐼 − 𝛿𝜃 0
𝐺≜ |∧ = 2
𝜕𝛿𝑥 𝛿𝑥
0 𝐼3