这一课解决最基础也最容易被软件掩盖的问题：同一条速度为什么会有三个不同的数字？输入是带坐标标签的向量、姿态和传感器安装位置；输出是正确的导航速度、姿态传播式和传感器点速度模型。后续动力学、EKF、位置控制和推进器分配，都以这里的约定为共同语言。不要先记旋转矩阵；先想清楚物理向量没有改变，改变的是用哪组三根基向量描述它。
开始前只需掌握这些
先读 00.1 向量与矩阵乘法 ；本课出现姿态求导时，再回看 00.2 导数 。第一遍只要求会解释 v n = R b n v b v^n=R_b^nv^b v n = R b n v b
符号 含义与维度 单位／坐标约定 n , b , c n,b,c n , b , c 三个右手坐标系 n n n b b b c c c R b n R_b^n R b n 3 × 3 3\times3 3 × 3 b b b n n n 无量纲；列向量是b b b n n n p n , v n p^n,v^n p n , v n 参考点位置、对地速度；各3 × 1 3\times1 3 × 1 m、m / s \mathrm{m/s} m/s n n n ω b = [ p r , q r , r r ] T \omega^b=[p_r,q_r,r_r]^T ω b = [ p r , q r , r r ] T 机体对n n n 3 × 1 3\times1 3 × 1 r a d / s \mathrm{rad/s} rad/s b b b p p p ϕ , θ , ψ ; q = [ q 0 , q v T ] T \phi,\theta,\psi;q=[q_0,q_v^T]^T ϕ , θ , ψ ; q = [ q 0 , q v T ] T 横滚俯仰偏航；标量在前Hamilton单位四元数 rad；q q q b → n b\to n b → n ℓ b \ell^b ℓ b 参考点到传感器的固定杆臂；3 × 1 3\times1 3 × 1 m；b b b
从基向量投影到可检查的旋转矩阵
假设研究区域足够小，可把导航系看成局部惯性系；姿态描述刚性机体，安装外参在本课保持常数。令机体三根单位轴在导航系中的坐标为矩阵的三列。将一个向量沿机体三轴展开，再对导航三轴取投影，就得到第一行等式。基轴正交使内积保持不变，所以逆矩阵是转置；右手性再要求行列式为正一，而不仅是正交。
a n = [ e b x n e b y n e b z n ] a b = R b n a b , ( R b n ) T R b n = I 3 , det R b n = 1 , R n b = ( R b n ) T . \begin{aligned}a^n&=[e_{b_x}^n\ e_{b_y}^n\ e_{b_z}^n]a^b=R_b^n a^b,\\(R_b^n)^TR_b^n&=I_3,\qquad\det R_b^n=1,\qquad R_n^b=(R_b^n)^T.\end{aligned} a n ( R b n ) T R b n = [ e b x n e b y n e b z n ] a b = R b n a b , = I 3 , det R b n = 1 , R n b = ( R b n ) T . (1.1)
把式( 1.1 ) (1.1) ( 1.1 )
这里的 a a a e b x n e_{b_x}^n e b x n
例如 a b = ( 1 , 2 , 3 ) T a^b=(1,2,3)^T a b = ( 1 , 2 , 3 ) T ( 0 , 1 , 0 ) T (0,1,0)^T ( 0 , 1 , 0 ) T ( − 2 , 0 , 0 ) T (-2,0,0)^T ( − 2 , 0 , 0 ) T ( 0 , 0 , 3 ) T (0,0,3)^T ( 0 , 0 , 3 ) T a n = ( − 2 , 1 , 3 ) T a^n=(-2,1,3)^T a n = ( − 2 , 1 , 3 ) T 14 14 14
采用ZYX姿态参数化：先以偏航确定水平朝向，再俯仰，再横滚。下面矩阵乘法从右向左作用于向量；不能把乘法顺序与口头描述的姿态构造混为一谈。记号c c c s s s
R x ( ϕ ) = [ 1 0 0 0 c ϕ − s ϕ 0 s ϕ c ϕ ] , R y ( θ ) = [ c θ 0 s θ 0 1 0 − s θ 0 c θ ] , R z ( ψ ) = [ c ψ − s ψ 0 s ψ c ψ 0 0 0 1 ] , R b n = R z R y R x = [ c ψ c θ c ψ s θ s ϕ − s ψ c ϕ c ψ s θ c ϕ + s ψ s ϕ s ψ c θ s ψ s θ s ϕ + c ψ c ϕ s ψ s θ c ϕ − c ψ s ϕ − s θ c θ s ϕ c θ c ϕ ] . \begin{aligned}R_x(\phi)&=\begin{bmatrix}1&0&0\\0&c_\phi&-s_\phi\\0&s_\phi&c_\phi\end{bmatrix},\quad R_y(\theta)=\begin{bmatrix}c_\theta&0&s_\theta\\0&1&0\\-s_\theta&0&c_\theta\end{bmatrix},\\R_z(\psi)&=\begin{bmatrix}c_\psi&-s_\psi&0\\s_\psi&c_\psi&0\\0&0&1\end{bmatrix},\\R_b^n&=R_zR_yR_x=\begin{bmatrix}c_\psi c_\theta&c_\psi s_\theta s_\phi-s_\psi c_\phi&c_\psi s_\theta c_\phi+s_\psi s_\phi\\s_\psi c_\theta&s_\psi s_\theta s_\phi+c_\psi c_\phi&s_\psi s_\theta c_\phi-c_\psi s_\phi\\-s_\theta&c_\theta s_\phi&c_\theta c_\phi\end{bmatrix}.\end{aligned} R x ( ϕ ) R z ( ψ ) R b n = 1 0 0 0 c ϕ s ϕ 0 − s ϕ c ϕ , R y ( θ ) = c θ 0 − s θ 0 1 0 s θ 0 c θ , = c ψ s ψ 0 − s ψ c ψ 0 0 0 1 , = R z R y R x = c ψ c θ s ψ c θ − s θ c ψ s θ s ϕ − s ψ c ϕ s ψ s θ s ϕ + c ψ c ϕ c θ s ϕ c ψ s θ c ϕ + s ψ s ϕ s ψ s θ c ϕ − c ψ s ϕ c θ c ϕ . (1.2)
为什么式( 1.2 ) (1.2) ( 1.2 )
绕向下轴正转时，前轴朝右转，因此偏航矩阵第一列的东向分量是 + s i n ψ +sin\psi + s in ψ − sin θ -\sin\theta − sin θ
完整乘法可以手算一个元素：乘积第一行第二列先取 R y R x R_yR_x R y R x ( sin θ sin ϕ , cos ϕ , cos θ sin ϕ ) T (\sin\theta\sin\phi,\cos\phi,\cos\theta\sin\phi)^T ( sin θ sin ϕ , cos ϕ , cos θ sin ϕ ) T R z R_z R z ( cos ψ , − sin ψ , 0 ) (\cos\psi,-\sin\psi,0) ( cos ψ , − sin ψ , 0 ) cos ψ sin θ sin ϕ − sin ψ cos ϕ \cos\psi\sin\theta\sin\phi-\sin\psi\cos\phi cos ψ sin θ sin ϕ − sin ψ cos ϕ 90 90 90 std::sin 并不是九十度。
手算：水平、向东即偏航90度时，机体前速0.2 m / s \mathrm{m/s} m/s m / s \mathrm{m/s} m/s
R b n ( 0 , 0 , π / 2 ) = [ 0 − 1 0 1 0 0 0 0 1 ] , R c b = [ 0 0 1 1 0 0 0 1 0 ] , R c n = R b n R c b . R_b^n(0,0,\pi/2)=\begin{bmatrix}0&-1&0\\1&0&0\\0&0&1\end{bmatrix},\qquad R_c^b=\begin{bmatrix}0&0&1\\1&0&0\\0&1&0\end{bmatrix},\qquad R_c^n=R_b^nR_c^b. R b n ( 0 , 0 , π /2 ) = 0 1 0 − 1 0 0 0 0 1 , R c b = 0 1 0 0 0 1 1 0 0 , R c n = R b n R c b . (1.3)
式( 1.3 ) (1.3) ( 1.3 )
R c b R_c^b R c b R b n R_b^n R b n R b n R c b R_b^nR_c^b R b n R c b ( 0 , 0 , 1 ) T (0,0,1)^T ( 0 , 0 , 1 ) T ( 1 , 0 , 0 ) T (1,0,0)^T ( 1 , 0 , 0 ) T ( 0 , 1 , 0 ) T (0,1,0)^T ( 0 , 1 , 0 ) T
先把叉乘矩阵按一行一行乘开
后面的 [ ω b ] × [\omega^b]_\times [ ω b ] × 00.1 的叉乘表 ，没有额外定义。若只绕下轴转，ω b = [ 0 , 0 , r r ] T \omega^b=[0,0,r_r]^T ω b = [ 0 , 0 , r r ] T
[ ω b ] × = [ 0 − r r 0 r r 0 0 0 0 0 ] , [ ω b ] × ℓ b = [ − r r ℓ y r r ℓ x 0 ] . [\omega^b]_\times=\begin{bmatrix}0&-r_r&0\\r_r&0&0\\0&0&0\end{bmatrix},
\qquad
[\omega^b]_\times\ell^b=\begin{bmatrix}-r_r\ell_y\\r_r\ell_x\\0\end{bmatrix}. [ ω b ] × = 0 r r 0 − r r 0 0 0 0 0 , [ ω b ] × ℓ b = − r r ℓ y r r ℓ x 0 . ℓ x , ℓ y \ell_x,\ell_y ℓ x , ℓ y r r r_r r r r r = 0.5 r a d / s r_r=0.5\ \mathrm{rad/s} r r = 0.5 rad/s ℓ b = [ 0.2 , 0 , 0 ] T m \ell^b=[0.2,0,0]^T\ \mathrm m ℓ b = [ 0.2 , 0 , 0 ] T m 0.1 m / s 0.1\ \mathrm{m/s} 0.1 m/s
欧拉角变化率不是陀螺仪的三个读数
陀螺仪输出的是瞬时机体角速度。欧拉角变化率却分别绕三个不同的中间轴发生。将偏航轴、俯仰轴和横滚轴都投到最终机体系，再相加，得到下式左矩阵。对其求逆便得到右侧的T T T
ω b = [ 1 0 − s θ 0 c ϕ s ϕ c θ 0 − s ϕ c ϕ c θ ] [ ϕ ˙ θ ˙ ψ ˙ ] , [ ϕ ˙ θ ˙ ψ ˙ ] = T ( ϕ , θ ) ω b , T = [ 1 s ϕ tan θ c ϕ tan θ 0 c ϕ − s ϕ 0 s ϕ / c θ c ϕ / c θ ] . \begin{aligned}\omega^b&=\begin{bmatrix}1&0&-s_\theta\\0&c_\phi&s_\phi c_\theta\\0&-s_\phi&c_\phi c_\theta\end{bmatrix}\begin{bmatrix}\dot\phi\\\dot\theta\\\dot\psi\end{bmatrix},\\\begin{bmatrix}\dot\phi\\\dot\theta\\\dot\psi\end{bmatrix}&=T(\phi,\theta)\omega^b,\quad T=\begin{bmatrix}1&s_\phi\tan\theta&c_\phi\tan\theta\\0&c_\phi&-s_\phi\\0&s_\phi/c_\theta&c_\phi/c_\theta\end{bmatrix}.\end{aligned} ω b ϕ ˙ θ ˙ ψ ˙ = 1 0 0 0 c ϕ − s ϕ − s θ s ϕ c θ c ϕ c θ ϕ ˙ θ ˙ ψ ˙ , = T ( ϕ , θ ) ω b , T = 1 0 0 s ϕ tan θ c ϕ s ϕ / c θ c ϕ tan θ − s ϕ c ϕ / c θ . (1.4)
用两个标量方程亲手解出式( 1.4 ) (1.4) ( 1.4 )
先看机体系第二、第三行：q r = cos ϕ θ ˙ + sin ϕ cos θ ψ ˙ q_r=\cos\phi\dot\theta+\sin\phi\cos\theta\dot\psi q r = cos ϕ θ ˙ + sin ϕ cos θ ψ ˙ r r = − sin ϕ θ ˙ + cos ϕ cos θ ψ ˙ r_r=-\sin\phi\dot\theta+\cos\phi\cos\theta\dot\psi r r = − sin ϕ θ ˙ + cos ϕ cos θ ψ ˙ cos ϕ \cos\phi cos ϕ sin ϕ \sin\phi sin ϕ θ ˙ = cos ϕ q r − sin ϕ r r \dot\theta=\cos\phi q_r-\sin\phi r_r θ ˙ = cos ϕ q r − sin ϕ r r sin ϕ \sin\phi sin ϕ cos ϕ \cos\phi cos ϕ cos θ ψ ˙ = sin ϕ q r + cos ϕ r r \cos\theta\dot\psi=\sin\phi q_r+\cos\phi r_r cos θ ψ ˙ = sin ϕ q r + cos ϕ r r ψ ˙ \dot\psi ψ ˙ p r = ϕ ˙ − sin θ ψ ˙ p_r=\dot\phi-\sin\theta\dot\psi p r = ϕ ˙ − sin θ ψ ˙
例如横滚零、俯仰三十度，只测到机体偏航角速率 r r = 0.2 r a d / s r_r=0.2\ \mathrm{rad/s} r r = 0.2 rad/s ϕ ˙ ≈ 0.11547 \dot\phi\approx0.11547 ϕ ˙ ≈ 0.11547 θ ˙ = 0 \dot\theta=0 θ ˙ = 0 ψ ˙ ≈ 0.23094 r a d / s \dot\psi\approx0.23094\ \mathrm{rad/s} ψ ˙ ≈ 0.23094 rad/s T T T
为了避免欧拉角奇异，积分内部用单位四元数。令旋转轴为单位向量a a a α \alpha α q q q q q q q q q
[ a ] × = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] , q = [ cos ( α / 2 ) a sin ( α / 2 ) ] , R ( q ) = ( q 0 2 − q v T q v ) I 3 + 2 q v q v T + 2 q 0 [ q v ] × , q ˙ = 1 2 q ⊗ [ 0 , ( ω b ) T ] T = 1 2 [ 0 − ( ω b ) T ω b − [ ω b ] × ] q . \begin{aligned}[a]_\times&=\begin{bmatrix}0&-a_3&a_2\\a_3&0&-a_1\\-a_2&a_1&0\end{bmatrix},\quad q=\begin{bmatrix}\cos(\alpha/2)\\a\sin(\alpha/2)\end{bmatrix},\\R(q)&=(q_0^2-q_v^Tq_v)I_3+2q_vq_v^T+2q_0[q_v]_\times,\\\dot q&=\tfrac12q\otimes[0,(\omega^b)^T]^T=\tfrac12\begin{bmatrix}0&-(\omega^b)^T\\\omega^b&-[\omega^b]_\times\end{bmatrix}q.\end{aligned} [ a ] × R ( q ) q ˙ = 0 a 3 − a 2 − a 3 0 a 1 a 2 − a 1 0 , q = [ cos ( α /2 ) a sin ( α /2 ) ] , = ( q 0 2 − q v T q v ) I 3 + 2 q v q v T + 2 q 0 [ q v ] × , = 2 1 q ⊗ [ 0 , ( ω b ) T ] T = 2 1 [ 0 ω b − ( ω b ) T − [ ω b ] × ] q . (1.5)
式( 1.5 ) (1.5) ( 1.5 )
Hamilton乘法把两个四元数 ( s , x ) (s,x) ( s , x ) ( t , y ) (t,y) ( t , y ) ( s t − x T y , s y + t x + x × y ) (st-x^Ty,\ sy+tx+x\times y) ( s t − x T y , sy + t x + x × y ) s , t s,t s , t x , y x,y x , y ( q 0 , − q v ) (q_0,-q_v) ( q 0 , − q v ) ( 0 , a b ) (0,a^b) ( 0 , a b ) q q q a b a^b a b R ( q ) R(q) R ( q )
微分式中的 1 / 2 1/2 1/2 q = ( 2 / 2 , 0 , 0 , 2 / 2 ) T q=(\sqrt2/2,0,0,\sqrt2/2)^T q = ( 2 /2 , 0 , 0 , 2 /2 ) T − 1 -1 − 1 + 1 +1 + 1
最后一行来自一个短时间间隔内的增量四元数：标量部近似一，向量部近似角速度乘时间的一半。机体角速度对应右乘增量；若改成导航角速度，乘法方向也要改。数值上可以直接用有限角度增量右乘，然后归一化，而不是让欧拉积分逐渐破坏单位长度。
δ q = [ cos ( ∥ ω b ∥ Δ t / 2 ) ω b ∥ ω b ∥ sin ( ∥ ω b ∥ Δ t / 2 ) ] , q k + 1 = q k ⊗ δ q ∥ q k ⊗ δ q ∥ , R ˙ b n = R b n [ ω b ] × . \begin{aligned}\delta q&=\begin{bmatrix}\cos(\|\omega^b\|\Delta t/2)\\\dfrac{\omega^b}{\|\omega^b\|}\sin(\|\omega^b\|\Delta t/2)\end{bmatrix},\quad q_{k+1}=\frac{q_k\otimes\delta q}{\|q_k\otimes\delta q\|},\\\dot R_b^n&=R_b^n[\omega^b]_\times.\end{aligned} δ q R ˙ b n = cos ( ∥ ω b ∥Δ t /2 ) ∥ ω b ∥ ω b sin ( ∥ ω b ∥Δ t /2 ) , q k + 1 = ∥ q k ⊗ δ q ∥ q k ⊗ δ q , = R b n [ ω b ] × . (1.6)
式( 1.6 ) (1.6) ( 1.6 )
令 Δ t \Delta t Δ t α = ∥ ω b ∥ Δ t \alpha=\|\omega^b\|\Delta t α = ∥ ω b ∥Δ t
静止时应直接得到单位增量；以 1 r a d / s 1\ \mathrm{rad/s} 1 rad/s 0.1 s 0.1\ \mathrm{s} 0.1 s 0.99875026 0.99875026 0.99875026 0.04997917 0.04997917 0.04997917 ( 0.99500417 , 0.09983342 , 0 ) T (0.99500417,0.09983342,0)^T ( 0.99500417 , 0.09983342 , 0 ) T
零角速度时，增量四元数直接定义为 δ q = [ 1 , 0 , 0 , 0 ] T \delta q=[1,0,0,0]^T δ q = [ 1 , 0 , 0 , 0 ] T x = ∥ ω b ∥ Δ t / 2 x=\|\omega^b\|\Delta t/2 x = ∥ ω b ∥Δ t /2 ( Δ t / 2 ) sinc ( x ) ω b (\Delta t/2)\operatorname{sinc}(x)\omega^b ( Δ t /2 ) sinc ( x ) ω b sinc ( x ) = sin x / x \operatorname{sinc}(x)=\sin x/x sinc ( x ) = sin x / x sinc ( 0 ) = 1 \operatorname{sinc}(0)=1 sinc ( 0 ) = 1 1 − x 2 / 6 1-x^2/6 1 − x 2 /6
杆臂：从位置求导，不靠背补偿符号
同一刚体上参考点 O O O S S S
p S n = p O n + R b n ℓ b , v S n = v O n + R b n [ ω b ] × ℓ b , v S b = ( R b n ) T v O n + ω b × ℓ b , v O n = R b n ( v S b − ω b × ℓ b ) . \begin{aligned}p_S^n&=p_O^n+R_b^n\ell^b,\\v_S^n&=v_O^n+R_b^n[\omega^b]_\times\ell^b,\\v_S^b&=(R_b^n)^Tv_O^n+\omega^b\times\ell^b,\\v_O^n&=R_b^n(v_S^b-\omega^b\times\ell^b).\end{aligned} p S n v S n v S b v O n = p O n + R b n ℓ b , = v O n + R b n [ ω b ] × ℓ b , = ( R b n ) T v O n + ω b × ℓ b , = R b n ( v S b − ω b × ℓ b ) . (1.7)
式( 1.7 ) (1.7) ( 1.7 )
下标 O O O S S S O O O S S S R ℓ ˙ R\dot\ell R ℓ ˙ R T R^T R T
一个有效的程序测试是先正向合成传感器速度，再用补偿函数还原参考点速度。令 v O n = ( 0.1 , 0 , 0 ) T v_O^n=(0.1,0,0)^T v O n = ( 0.1 , 0 , 0 ) T ω b = ( 0 , 0 , 0.2 ) T \omega^b=(0,0,0.2)^T ω b = ( 0 , 0 , 0.2 ) T ( 0 , − 0.172 , 0 ) T (0,-0.172,0)^T ( 0 , − 0.172 , 0 ) T 0.1 m / s 0.1\ \mathrm{m/s} 0.1 m/s 0.172 m / s 0.172\ \mathrm{m/s} 0.172 m/s
使用项目杆臂示例( − 0.360 , 0 , 0.125 ) (-0.360,0,0.125) ( − 0.360 , 0 , 0.125 ) r a d / s \mathrm{rad/s} rad/s ( 0 , − 0.072 , 0 ) m / s (0,-0.072,0)\,\mathrm{m/s} ( 0 , − 0.072 , 0 ) m/s
离线算法、源码入口与手写练习
参考实现：C++17，先独立手写再展开
// 独立教学实现，C++17；单位m、s、rad，机体FRD，导航NED #include <array> #include <cmath> #include <cassert> #include <iostream> using V = std :: array < double , 3 >; V cross ( V a , V b ){ return {a[ 1 ] * b[ 2 ] - a[ 2 ] * b[ 1 ],a[ 2 ] * b[ 0 ] - a[ 0 ] * b[ 2 ],a[ 0 ] * b[ 1 ] - a[ 1 ] * b[ 0 ]};} V rz ( double psi , V v ){ double c = std :: cos (psi),s = std :: sin (psi); return {c * v[ 0 ] - s * v[ 1 ],s * v[ 0 ] + c * v[ 1 ],v[ 2 ]};} V referenceVelocity ( double psi , V sensor , V omega , V lever ){ V rotational = cross (omega,lever); for ( int i = 0 ;i < 3 ; ++ i)sensor[i] -= rotational[i]; return rz (psi,sensor); } using Q = std :: array < double , 4 >; // 标量在前，b到n Q multiply ( Q a , Q b ){ return {a[ 0 ] * b[ 0 ] - a[ 1 ] * b[ 1 ] - a[ 2 ] * b[ 2 ] - a[ 3 ] * b[ 3 ], a[ 0 ] * b[ 1 ] + a[ 1 ] * b[ 0 ] + a[ 2 ] * b[ 3 ] - a[ 3 ] * b[ 2 ], a[ 0 ] * b[ 2 ] - a[ 1 ] * b[ 3 ] + a[ 2 ] * b[ 0 ] + a[ 3 ] * b[ 1 ], a[ 0 ] * b[ 3 ] + a[ 1 ] * b[ 2 ] - a[ 2 ] * b[ 1 ] + a[ 3 ] * b[ 0 ]}; } Q propagate ( Q q , V w , double dt ){ double norm = std :: sqrt (w[ 0 ] * w[ 0 ] + w[ 1 ] * w[ 1 ] + w[ 2 ] * w[ 2 ]); double x = norm * dt / 2 ; double sinc = std :: abs (x) < 1 e- 6 ? 1 - x * x / 6 : std :: sin (x) / x; double k = dt * sinc / 2 ; Q next = multiply (q,{ std :: cos (x),k * w[ 0 ],k * w[ 1 ],k * w[ 2 ]}); double n = 0 ; for ( double v:next)n += v * v; for ( double & v:next)v /= std :: sqrt (n); return next; } int main (){ const double pi = std :: acos ( - 1.0 ); V v = referenceVelocity (pi / 2 ,{ 0 , - .172 , 0 },{ 0 , 0 , .2 },{ - .360 , 0 , .125 }); assert ( std :: abs (v[ 0 ] - .1 ) < 1 e- 12 && std :: abs (v[ 1 ]) < 1 e- 12 ); Q q = propagate ({ 1 , 0 , 0 , 0 },{ 0 , 0 , 1 }, .1 ); assert ( std :: abs (q[ 0 ] - std :: cos ( .05 )) < 1 e- 12 ); assert ( std :: abs (q[ 3 ] - std :: sin ( .05 )) < 1 e- 12 ); Q stationary = propagate ({ 1 , 0 , 0 , 0 },{ 0 , 0 , 0 }, .1 ); assert (stationary[ 0 ] == 1 && stationary[ 3 ] == 0 ); std ::cout << v[ 0 ] << " " << v[ 1 ] << " " << v[ 2 ] << " \n " ; }
项目阅读顺序：ArduSub/handle_dvl_data.cpp 的 dvl_data_BI 先核对设备轴向和m m / s \mathrm{mm/s} mm/s m / s \mathrm{m/s} m/s AP_AHRS_NavEKF::writeBodyFrameOdom 进入机体系里程计接口；libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp 的 FuseBodyVel 再查看预测和杆臂如何组合。报告中的创新采用预测减观测，本教材统一观测减预测，两者只是整套更新符号要一致。随后看 AP_InertialNav_NavEKF::update：NED米到NEU厘米需要第三轴反号并乘100。
练习一：手写矩阵乘法验证R T R = I 3 R^TR=I_3 R T R = I 3 m / s \mathrm{m/s} m/s ( 0 , − 1 , 0 ) m / s (0,-1,0)\,\mathrm{m/s} ( 0 , − 1 , 0 ) m/s ( 0.3 , 0.1 , 0 ) (0.3,0.1,0) ( 0.3 , 0.1 , 0 ) r a d / s \mathrm{rad/s} rad/s ( 0.04 , − 0.12 , 0 ) m / s (0.04,-0.12,0)\,\mathrm{m/s} ( 0.04 , − 0.12 , 0 ) m/s r a d / s \mathrm{rad/s} rad/s s s s q = ( cos 0.05 , 0 , 0 , sin 0.05 ) T q=(\cos0.05,0,0,\sin0.05)^T q = ( cos 0.05 , 0 , 0 , sin 0.05 ) T
TB01-S1 · Thor I. Fossen：MSS Rzyx.m
精读路线：逐项对照ZYX矩阵，再以零角和90度偏航检验列向量方向；本课其余推导由基变换和刚体求导展开。
原始C++逐行阅读：ArduSub 4.1.1怎样填旋转矩阵
下面是官方 Matrix3<T>::from_euler 中实际存在的三行，不是教学伪代码。来源：ArduPilot，ArduSub-4.1.1，固定提交 66c674c1927d5d31401e6b4bc48b8eb3ee7e066e；原文件第26—43行 。版权 Andrew Tridgell，2012；以下原文片段依 GNU GPL v3-or-later 提供，许可证 。
c.x = - sp; c.y = sr * cp; c.z = cr * cp; 函数开头将 sp、cp 定义为俯仰的正弦和余弦，将 sr、cr 定义为横滚的正弦和余弦；成员 c 是第三行。因此这三行就是 [ − sin θ , sin ϕ cos θ , cos ϕ cos θ ] [-\sin\theta,\sin\phi\cos\theta,\cos\phi\cos\theta] [ − sin θ , sin ϕ cos θ , cos ϕ cos θ ] ( 0 , 0 , 1 ) (0,0,1) ( 0 , 0 , 1 ) ( − 0.5 , 0 , 3 / 2 ) (-0.5,0,\sqrt3/2) ( − 0.5 , 0 , 3 /2 )
读完整函数时，逐项把 a.x、a.y、a.z 对应教材第一行，把 b 对应第二行，把 c 对应第三行。接着看同文件的 to_euler：它用第三行第一列恢复俯仰，用第三行第二、第三列恢复横滚，用第二行第一列与第一行第一列恢复偏航。正反转换使用的是同一约定。项目DVL安装轴交换属于进入这套姿态关系之前的传感器外参，不应通过修改这九个通用元素来修正某一台传感器安装。
零基础停步自检
若姿态不变、杆臂为零，传感器与参考点速度还需要杆臂补偿吗？
展开答案与原因
不需要。ω b × ℓ b = 0 \omega^b\times\ell^b=0 ω b × ℓ b = 0