公式(22)的
r
p
=
(
p
b
0
−
p
^
b
0
,
q
b
0
−
q
^
b
0
,
v
b
0
−
v
^
b
0
,
b
a
b
0
−
b
^
a
b
0
,
b
g
b
0
−
b
^
g
b
0
,
.
.
.
,
b
g
b
n
−
b
^
g
b
n
)
′
r_p=(p_{b_0}-\hat{p}_{b_0},q_{b_0}-\hat{q}_{b_0},v_{b_0}-\hat{v}_{b_0},b_{a_{b_0}}-\hat{b}_{a_{b_0}},b_{g_{b_0}}-\hat{b}_{g_{b_0}},...,b_{g_{b_n}}-\hat{b}_{g_{b_n}})'
rp=(pb0−p^b0,qb0−q^b0,vb0−v^b0,bab0−b^ab0,bgb0−b^gb0,...,bgbn−b^gbn)′
r B = ( α b k + 1 b k − α ^ b k + 1 b k , β b k + 1 b k − β ^ b k + 1 b k , γ b k b k + 1 ⊗ γ ? ? , b a − b ^ a , b g − b ^ g ) ′ r_{\textbf{B}}=(\alpha_{b_{k+1}}^{b_k}-\hat{\alpha}_{b_{k+1}}^{b_k},\beta_{b_{k+1}}^{b_k}-\hat{\beta}_{b_{k+1}}^{b_k},\gamma_{b_k}^{b_{k+1}}\otimes\gamma_{?}^{?},b_a-\hat{b}_a,b_g-\hat{b}_g)' rB=(αbk+1bk−α^bk+1bk,βbk+1bk−β^bk+1bk,γbkbk+1⊗γ??,ba−b^a,bg−b^g)′
r
C
=
(
u
1
−
u
0
′
,
v
1
−
v
0
′
,
.
.
.
,
u
i
−
u
j
′
,
v
i
−
v
j
′
,
.
.
.
)
′
r_{\textbf{C}}=(u_1-u_0',v_1-v_0',...,u_i-u_j',v_i-v_j',...)'
rC=(u1−u0′,v1−v0′,...,ui−uj′,vi−vj′,...)′
其中,
(
u
i
,
v
i
)
=
π
−
1
(
u
i
,
v
i
)
(u_i,v_i)=\pi^{-1}(u_i,v_i)
(ui,vi)=π−1(ui,vi)
(
u
j
′
,
v
j
′
)
=
R
b
c
(
(
R
c
i
w
(
R
c
i
w
(
?
?
)
+
p
?
)
+
p
?
)
+
p
Δ
)
(u_j',v_j')=R_{b}^{c}((R_{c_i}^{w}(R_{c_i}^{w}(??)+p_{?})+p_{?})+p_{\Delta})
(uj′,vj′)=Rbc((Rciw(Rciw(??)+p?)+p?)+pΔ)这个需要画个图