组合导航原理（四）——等效旋转向量
在t ~ t+Δt时间内，必须定轴旋转的解决思路：
（1）如果传感器带来的误差 > 不可交换性带来的误差，可以直接默认可交换性条件成立。
（2）提高采样率，使得Δt足够小，小到旋转轴来不及变化
IMU 输出的是 Δθ = [ Δθx, Δθy, Δθz ] T , Δθ 是在t + Δt 相对于时刻的 三轴角增量
问题1：如果使用方向余弦矩阵 Cb(t)b(t + Δt) 如何决定先绕哪个轴，后绕哪个轴来计算？
（不同的绕法，有不同的结果）
当t与 t + Δt 时间内，Δθ = [Δθx, Δθy, Δθz]T足够小，有：
也就是：
当t与 t + Δt 时间内，Δθ = [Δθx, Δθy, Δθz]T足够小，C = I + Δθ X
Δθ = [Δθx, Δθy, Δθz]T
* 顺序依然是 [滚转roll（Φ），俯仰pitch（θ），偏航yaw（Ψ）]
* 那么是不是安装的时候，x轴要沿着航向的方向放置？？?
说明：
这是使用【可交换性条件】下，使用角增量进行姿态更新。
这是使用【等效旋转向量】下，使用角增量进行姿态更新。
虽然都长得差不多，但是ϕm是经过数学推导，考虑了【可交换性条件】不成立的情况的。
等效旋转矢量ϕ
如果找到b系，在（ t ~ t + Δt）的等效旋转矢量ϕ， 其转角|| ϕ || 依然是微小量，但是不同于：
Cb(t)b(t+Δt)≈ I + ΔθX ，
效旋转矢量ϕ 是严格数学推导的，最终可以用于：
Cb(t)b(t+Δt) = I + sin|| ϕ || *μX + （1-cos|| ϕ || )(μX)²
μ = ϕ / || ϕ ||
或者：
qb(t)b(t+Δt) = cos(|| ϕ || / 2) + ϕ / || ϕ || *sin(|| ϕ || / 2)
最后，可以用：
Ci b(t+Δt) = Ci b(t) Cb(t)b(t+Δt)
总体思路：
（1）对ϕ 进行微分（求导），得到ϕ‘
（2）对ϕ‘ 进行积分，得到ϕ
等效旋转向量微分
b 系和 R 系相对姿态对应的四元数记作 qRb ，可写作如下三角函数形式：
为书写方便我们将省略四元数 qRb的上下标，简写为 q，角速度向量 ωbRb 简写 为 ω，等效旋转矢量 ϕRb 简写为 ϕ。
将上述四元数记作
（1）q = f1 + f2ϕ
（2）f1 ≡ cos (ϕ / 2)
（3）f2 ≡ 1 / ϕ * sin(ϕ / 2)
回顾一下：
q' = 1 / 2 q ◦ ω
* 虽然qb(t)b(t+Δt) = [ 1, 1 / 2 Δθ ]，Δθ 是发生在 t ~ t+Δt，但是对Δθ微分后，就不用一定是在t ~ t+Δt的角速度了。
* ω = [0 , ωbRb,x ,ωbRb,y ,ωbRb,z ] T
q' = 1 /2 f1 * ω + 1 / 2 * f2 * ϕ ◦ ω
又：
pv ◦ qv = (0 + p1i + p2j + p3k) ◦ (0 + q1i + q2j + q3k)
= −(p1q1 + p2q2 + p3q3) + (p2q3 − p3q2)i + (p3q1 − p1q3)j + (p1q2 − p2q1)k
= −pvTqv + pv X qv
q' = 1 / 2 * f1 * ω + 1 / 2 * f2 ( ϕ × ω − ϕTω )
对q = f1 + f2ϕ 求导：
（1）q' = f1' + f2'ϕ + f2ϕ'
（2）f1' = − 1/2 * sin(ϕ / 2) * ϕ' = [ 1 / ϕ * sin(ϕ / 2) ] *ϕ * -1/2 ϕ' = -1/2 * f2*ϕϕ'
（3）f2' = 1 / 2 cos (ϕ / 2) * ϕ' / ϕ − sin (ϕ / 2) * ϕ' / ϕ² = ϕ' / ϕ * (1 / 2 * f1 - f2 )
q' = - 1 / 2 * ϕϕ'f2 + ϕ'/ ϕ (1/2f1 - f2) ϕ + f2ϕ'
两个式子相等，整理得：
ϕ' = 1 / 2 * f1 / f2 * ω + 1 / 2 * (ϕ x ω) - ϕ'/ ϕ * (1 / 2 * f1 / f2 - 1) ϕ + 1 / 2 * ϕ ϕ' - 1 /2 * ϕTω
因为 ϕ' 还是一个向量，但是1 / 2 * ϕ ϕ' - 1 /2 * ϕTω 是标量，因此：
1 / 2 * ϕ ϕ' - 1 /2 * ϕTω = 0
ϕ' / ϕ = 1 /(ϕ²)*ϕTω 代入：
ϕ' = 1 / 2 * f1 / f2 * ω + 1 / 2 (ϕ × ω) − 1 / ϕ² (1/ 2* f1/ f2 − 1 )(ϕTω)ϕ
三重矢积公式
(ϕ Tω)ϕ
= ϕ × (ϕ × ω) + ϕ Tϕω
= ϕ × (ϕ × ω）+ ϕ² ω
ϕ˙ = ω + 1 /2 (ϕ × ω) + 1/ ϕ² ( 1 − 1 /2 * f1 /f2 ) ϕ × (ϕ × ω)
将f1、f2回代，得到：
ϕ' = ω + 1 /2 (ϕ × ω) + 1 /ϕ² [ 1 − ϕ* sin* ϕ / 2(1 − cos ϕ) ] ϕ x( ϕ x ω)
在 上一节中，有：
（1）在绕旋转单位向量μ，旋转小角度Φ， [ Δθx,Δθy,Δθz]T 也是小值的时候
Φ*μ = Δθ。 C ≈ I + Φ*μ X = I + ΔθX
（2）换句话说，Δθ = [ Δθx,Δθy,Δθz]T 是旋转向量 μ，模长为Φ = ||Δθ|| 。
（3） Φ = ||Δθ|| ，Φ就是绕μ旋转的角度。
（4）最后可以写为：||Δθ||*μ =Δθ ，μ = Δθ / ||Δθ||
所以，在转角 || ϕ || 很小的时候，旋转向量可以认为是 ϕ = [ Δθx,Δθy,Δθz]T ，
ϕ‘ = [ ωx,ωy,ωz]T = ω，这么说，角速度也是一个向量！
而上式，相当于给ϕ‘ =ω 加了改正项
等效旋转向量积分
注意，因为 b在T = tk-1时刻，对比b在T = t 时还没发生旋转，所以 ϕ( tk-1) = 0
因此，不要忘记了，下面所计算的，是b在 tk-1 ~ tk时刻的等效旋转矢量：
Δθ(t) 也就是IMU在 T = t时的输出
那么，上式等号右侧依然有ϕ(t)，所以要按照迭代积分：
因为在（tk-1,tk）内，ϕ(t) 分量都是微小值，因此，上式：
ϕtt-Δt = Δθ(t) + 1 / 2 * ∫t-Δt t Δθ(τ) X ω(τ) dτ
(对比一下，非常眼熟！)
tk-1 = t - Δt
如何使用等效旋转向量步骤：
（1）假设( t - Δt, t ) 之间的角速度函数 ω(τ) = ω0 、 ω(τ) = a + b(t - tk-1) ... 等等
（可以让时间归零，让时间 t∈[0, t - tk-1]）
（2）求得Δθ (τ) = ∫t-Δt t ω(τ) dτ 函数表达式，注意 Δθ (τ) 是表达式，不需要求得其结果
（3）代入ϕtt-Δt = Δθ(t) + 1 / 2 *∫t-Δt t Δθ(τ) X ω(τ) dτ ， 算的其结果
总体思路：
（1）IMU输出的是t时刻，相对于上一时刻的角速度增量，Δθ(t) = [Δθx，Δθy，Δθz] T
（2）利用Δθ(tk-2)、Δθ(tk-1)、Δθ(t) , 模拟出在 t ∈ [tk-1, t] 时刻的角速度ω(τ)
（3）使用ω(τ)的函数，对Δθ(t)添加修正量
（4）计算旋转向量或四元素
单子样： （tk-1,tk），或（t - Δt, t）时间内，ω(τ) = ω0
ϕtt-Δt = Δθ(t) + 1 / 2 *∫t-Δt t Δθ(τ) X ω(τ) dτ
Δθ (τ) = ∫t-Δt t ω0 dτ = Δt ω0
回代
ϕtt-Δt = Δθ(t) + 1 / 2 *∫tt-Δt Δθ(τ) X ω(τ) dτ
= Δθ(t) +1 / 2 * ∫t t+Δt Δt ω0 X ω0 dτ
= Δθ(t)
因为ω0 X ω0 = 0 结果相当于对IMU的输出，没有添加任何改正，就作为ϕtt-Δt
双字样
