ω
=
1
2
(
(
ω
b
k
+
n
k
g
?
b
k
g
)
+
(
w
b
k
+
1
+
n
k
+
1
g
?
b
k
+
1
g
)
)
\omega = \frac{1}{2}((\omega_b^k+n_k^g-b_k^g)+(w_b^{k+1}+n_{k+1}^g-b_{k+1}^g))
ω=21?((ωbk?+nkg??bkg?)+(wbk+1?+nk+1g??bk+1g?))
其中，
w
b
k
w_b^k
wbk?为
k
k
k时刻下body坐标系的角速度，
n
k
g
n_k^g
nkg?为
k
k
k时刻下陀螺仪白噪声，
b
k
g
b_k^g
bkg?为
k
k
k时刻下陀螺仪偏置量。
n
k
a
n_k^a
nka?为
k
k
k时刻下加速度白噪声，
b
k
a
b_k^a
bka?为
k
k
k时刻下加速度偏置量。
k
+
1
k+1
k+1时刻下记号同理。
q
b
i
b
k
+
1
=
q
b
i
b
k
?
[
1
,
1
2
ω
δ
t
]
T
q_{b_i b_{k+1}} = q_{b_i b_k} \otimes [1, \frac{1}{2} \omega \delta t]^T
qbi?bk+1??=qbi?bk???[1,21?ωδt]T
a
=
1
2
(
q
b
i
b
k
(
a
b
k
+
n
b
k
?
b
k
a
)
+
q
b
i
b
k
+
1
(
a
b
k
+
1
+
n
b
k
+
1
?
b
k
+
1
a
)
)
a = \frac{1}{2}(q_{b_i b_{k}} (a_b^k + n_b^k -b_k^a) + q_{b_i b_{k+1}} (a_b^{k+1} + n_b^{k+1} - b_{k+1}^a))
a=21?(qbi?bk??(abk?+nbk??bka?)+qbi?bk+1??(abk+1?+nbk+1??bk+1a?))
α
b
i
b
k
+
1
=
α
b
i
b
k
+
β
b
i
b
k
δ
t
+
1
2
a
δ
t
2
\alpha_{b_i b_{k+1}} = \alpha_{b_i b_{k}} + \beta_{b_i b_k} \delta t + \frac{1}{2}a \delta t^2
αbi?bk+1??=αbi?bk??+βbi?bk??δt+21?aδt2
β
b
i
b
k
+
1
=
β
b
i
b
k
+
a
δ
t
\beta_{b_i b_{k+1}} = \beta_{b_i b_{k}} + a\delta t
βbi?bk+1??=βbi?bk??+aδt
b
k
+
1
a
=
b
k
a
+
n
b
k
a
δ
t
b_{k+1}^a = b_k^a + n_{b_k^a}\delta t
bk+1a?=bka?+nbka??δt
b
k
+
1
g
=
b
k
g
+
n
b
k
g
δ
t
b_{k+1}^g = b_k^g + n_{b_k^g}\delta t
bk+1g?=bkg?+nbkg??δt
f
15
=
δ
α
b
i
b
k
+
1
δ
b
k
g
f_{15} = \frac{\delta \alpha_{b_i b_{k+1}}}{\delta b_k^g}
f15?=δbkg?δαbi?bk+1???
由上面的递推公式可知
α
b
i
b
k
+
1
=
α
b
i
b
k
+
β
b
i
b
k
δ
t
+
1
2
a
δ
t
2
\alpha_{b_i b_{k+1}} = \alpha_{b_i b_{k}} + \beta_{b_i b_k} \delta t + \frac{1}{2}a \delta t^2
αbi?bk+1??=αbi?bk??+βbi?bk??δt+21?aδt2
其中，
α
b
i
b
k
\alpha_{b_i b_{k}}
αbi?bk??、
β
b
i
b
k
δ
t
\beta_{b_i b_k}\delta t
βbi?bk??δt都与
b
k
g
b_k^g
bkg?无关，可以省略，而很容易看出
a
a
a中含有
q
b
i
b
k
+
1
q_{b_i b_{k+1}}
qbi?bk+1??项，其中进一步含有对
b
k
g
b_k^g
bkg?相关的元素，必须保留。因此进一步推得
f
15
=
δ
1
2
a
δ
t
2
δ
b
k
g
f_{15} = \frac{\delta \frac{1}{2} a \delta t^2}{\delta b_k^g}
f15?=δbkg?δ21?aδt2?
其中，
a
=
1
2
(
q
b
i
b
k
(
a
b
k
+
n
b
k
?
b
k
a
)
+
q
b
i
b
k
+
1
(
a
b
k
+
1
+
n
b
k
+
1
?
b
k
+
1
a
)
)
a = \frac{1}{2}(q_{b_i b_{k}} (a_b^k + n_b^k -b_k^a) + q_{b_i b_{k+1}} (a_b^{k+1} + n_b^{k+1} - b_{k+1}^a))
a=21?(qbi?bk??(abk?+nbk??bka?)+qbi?bk+1??(abk+1?+nbk+1??bk+1a?))
q
b
i
b
k
(
a
b
k
+
n
b
k
?
b
k
a
)
q_{b_i b_{k}} (a_b^k + n_b^k -b_k^a)
qbi?bk??(abk?+nbk??bka?)依然与
b
k
g
b_k^g
bkg?无关，可以省略。
f
15
=
δ
1
4
q
b
i
b
k
+
1
(
a
b
k
+
1
+
n
b
k
+
1
?
b
k
+
1
a
)
δ
t
2
δ
b
k
g
f_{15}=\frac{\delta \frac{1}{4} q_{b_i b_{k+1}} (a_b^{k+1} + n_b^{k+1} - b_{k+1}^a) \delta t^2}{\delta b_k^g}
f15?=δbkg?δ41?qbi?bk+1??(abk+1?+nbk+1??bk+1a?)δt2?
白噪声项不可知，拿掉
f
15
=
δ
1
4
q
b
i
b
k
+
1
(
a
b
k
+
1
?
b
k
+
1
a
)
δ
t
2
δ
b
k
g
f_{15}=\frac{\delta \frac{1}{4} q_{b_i b_{k+1}} (a_b^{k+1} - b_{k+1}^a) \delta t^2}{\delta b_k^g}
f15?=δbkg?δ41?qbi?bk+1??(abk+1??bk+1a?)δt2?
f
15
=
δ
1
4
q
b
i
b
k
+
1
(
a
b
k
+
1
?
b
k
+
1
a
)
δ
t
2
δ
b
k
g
f_{15}=\frac{\delta \frac{1}{4} q_{b_i b_{k+1}} (a_b^{k+1} - b_{k+1}^a) \delta t^2}{\delta b_k^g}
f15?=δbkg?δ41?qbi?bk+1??(abk+1??bk+1a?)δt2?
q
b
i
b
k
+
1
=
q
b
i
b
k
?
[
1
,
1
2
ω
δ
t
]
T
q_{b_i b_{k+1}} = q_{b_i b_k} \otimes [1, \frac{1}{2} \omega \delta t]^T
qbi?bk+1??=qbi?bk???[1,21?ωδt]T
f
15
=
δ
1
4
q
b
i
b
k
?
[
1
,
1
2
ω
δ
t
]
T
(
a
b
k
+
1
?
b
k
+
1
a
)
δ
t
2
δ
b
k
g
f_{15}=\frac{\delta \frac{1}{4} q_{b_i b_k} \otimes [1, \frac{1}{2} \omega \delta t]^T (a_b^{k+1} - b_{k+1}^a) \delta t^2}{\delta b_k^g}
f15?=δbkg?δ41?qbi?bk???[1,21?ωδt]T(abk+1??bk+1a?)δt2?
其中
ω
=
1
2
(
(
ω
b
k
+
n
k
g
?
b
k
g
)
+
(
w
b
k
+
1
+
n
k
+
1
g
?
b
k
+
1
g
)
)
\omega = \frac{1}{2}((\omega_b^k+n_k^g-b_k^g)+(w_b^{k+1}+n_{k+1}^g-b_{k+1}^g))
ω=21?((ωbk?+nkg??bkg?)+(wbk+1?+nk+1g??bk+1g?))
去除不可知的白噪声项
ω
=
1
2
(
(
ω
b
k
?
b
k
g
)
+
(
w
b
k
+
1
?
b
k
+
1
g
)
)
\omega = \frac{1}{2}((\omega_b^k-b_k^g)+(w_b^{k+1}-b_{k+1}^g))
ω=21?((ωbk??bkg?)+(wbk+1??bk+1g?))
由于
k
+
1
k+1
k+1时刻的信息并不知道，在此处如果不使用中值积分，直接使用初始值，有
ω
=
ω
b
k
?
b
k
g
\omega =\omega_b^k-b_k^g
ω=ωbk??bkg?
f
15
=
δ
1
4
q
b
i
b
k
?
[
1
,
1
2
(
ω
b
k
?
b
k
g
)
δ
t
]
T
(
a
b
k
+
1
?
b
k
+
1
a
)
δ
t
2
δ
b
k
g
f_{15}=\frac{\delta \frac{1}{4} q_{b_i b_k} \otimes [1, \frac{1}{2} (\omega_b^k-b_k^g) \delta t]^T (a_b^{k+1} - b_{k+1}^a) \delta t^2}{\delta b_k^g}
f15?=δbkg?δ41?qbi?bk???[1,21?(ωbk??bkg?)δt]T(abk+1??bk+1a?)δt2?
此时，为了便于计算，我们需要把四元数表示旋转转换为用旋转矩阵表示矩阵的旋转，得到
f
15
=
1
4
δ
R
b
i
b
k
exp
?
(
(
(
w
b
k
?
b
k
g
)
δ
t
)
∧
)
(
a
b
k
+
1
?
b
k
+
1
a
)
δ
t
2
δ
b
k
g
f_{15}=\frac{1}{4} \frac{\delta R_{b_i b_k} \exp(((w_b^k-b_k^g)\delta t)^{\wedge})(a_b^{k+1} - b_{k+1}^a)\delta t^2}{\delta b_k^g}
f15?=41?δbkg?δRbi?bk??exp(((wbk??bkg?)δt)∧)(abk+1??bk+1a?)δt2?
观察式子，我们要想办法把
b
k
g
b_k^g
bkg?拆出来。回顾上一章，李代数旋转有性质
l
n
(
R
e
x
p
(
?
∧
)
)
∨
=
l
n
(
R
)
∨
+
J
r
?
1
?
ln(Rexp(\phi^{\land}))^{\vee}=ln(R)^{\vee}+J_r^{-1}\phi
ln(Rexp(?∧))∨=ln(R)∨+Jr?1??
类似的，对于非对数情况，有
exp
?
(
(
?
+
δ
?
)
∧
)
=
exp
?
(
?
∧
)
exp
?
(
(
J
r
(
?
)
δ
?
)
∧
)
\exp( (\phi + \delta\phi)^{\wedge} )= \exp(\phi^{\wedge})\exp((J_r(\phi)\delta\phi)^{\wedge})
exp((?+δ?)∧)=exp(?∧)exp((Jr?(?)δ?)∧)
lim
?
?
→
0
J
r
(
?
)
=
I
\lim_{\phi \rightarrow 0} J_r(\phi)=I
?→0lim?Jr?(?)=I
exp
?
(
(
(
w
b
k
?
b
k
g
)
δ
t
)
∧
=
exp
?
(
(
w
b
k
δ
t
)
∧
)
exp
?
(
(
J
r
(
w
b
k
δ
t
)
(
?
b
k
g
δ
t
)
)
∧
)
\exp(((w_b^k-b_k^g)\delta t)^{\wedge}=\exp((w_b^k\delta t)^{\wedge})\exp((J_r(w_b^k\delta t)(-b_k^g \delta t))^{\wedge})
exp(((wbk??bkg?)δt)∧=exp((wbk?δt)∧)exp((Jr?(wbk?δt)(?bkg?δt))∧)
f
15
=
1
4
δ
R
b
i
b
k
exp
?
(
(
(
w
b
k
?
b
k
g
)
δ
t
)
∧
)
(
a
b
k
+
1
?
b
k
+
1
a
)
δ
t
2
δ
b
k
g
f_{15}=\frac{1}{4} \frac{\delta R_{b_i b_k} \exp(((w_b^k-b_k^g)\delta t)^{\wedge})(a_b^{k+1} - b_{k+1}^a)\delta t^2}{\delta b_k^g}
f15?=41?δbkg?δRbi?bk??exp(((wbk??bkg?)δt)∧)(abk+1??bk+1a?)δt2?
f
15
=
1
4
δ
R
b
i
b
k
exp
?
(
(
w
b
k
δ
t
)
∧
)
exp
?
(
(
J
r
(
w
b
k
δ
t
)
(
?
b
k
g
δ
t
)
)
∧
)
(
a
b
k
+
1
?
b
k
+
1
a
)
δ
t
2
δ
b
k
g
f_{15}=\frac{1}{4} \frac{\delta R_{b_i b_k} \exp((w_b^k\delta t)^{\wedge})\exp((J_r(w_b^k\delta t)(-b_k^g \delta t))^{\wedge})(a_b^{k+1} - b_{k+1}^a)\delta t^2}{\delta b_k^g}
f15?=41?δbkg?δRbi?bk??exp((wbk?δt)∧)exp((Jr?(wbk?δt)(?bkg?δt))∧)(abk+1??bk+1a?)δt2?
w
b
k
δ
t
→
0
w_b^k\delta t \rightarrow0
wbk?δt→0
f
15
=
1
4
δ
R
b
i
b
k
exp
?
(
(
J
r
(
w
b
k
δ
t
)
(
?
b
k
g
δ
t
)
)
∧
)
(
a
b
k
+
1
?
b
k
+
1
a
)
δ
t
2
δ
b
k
g
f_{15}=\frac{1}{4} \frac{\delta R_{b_i b_k} \exp((J_r(w_b^k\delta t)(-b_k^g \delta t))^{\wedge})(a_b^{k+1} - b_{k+1}^a)\delta t^2}{\delta b_k^g}
f15?=41?δbkg?δRbi?bk??exp((Jr?(wbk?δt)(?bkg?δt))∧)(abk+1??bk+1a?)δt2?
f
15
=
1
4
δ
R
b
i
b
k
exp
?
(
(
?
b
k
g
δ
t
)
)
∧
)
(
a
b
k
+
1
?
b
k
+
1
a
)
δ
t
2
δ
b
k
g
f_{15}=\frac{1}{4} \frac{\delta R_{b_i b_k} \exp((-b_k^g \delta t))^{\wedge})(a_b^{k+1} - b_{k+1}^a)\delta t^2}{\delta b_k^g}
f15?=41?δbkg?δRbi?bk??exp((?bkg?δt))∧)(abk+1??bk+1a?)δt2?
f
15
=
1
4
δ
R
b
i
b
k
(
I
+
(
?
b
k
g
δ
t
)
)
∧
)
(
a
b
k
+
1
?
b
k
+
1
a
)
δ
t
2
δ
b
k
g
f_{15}=\frac{1}{4} \frac{\delta R_{b_i b_k} (I+(-b_k^g \delta t))^{\wedge})(a_b^{k+1} - b_{k+1}^a)\delta t^2}{\delta b_k^g}
f15?=41?δbkg?δRbi?bk??(I+(?bkg?δt))∧)(abk+1??bk+1a?)δt2?
f
15
=
1
4
δ
R
b
i
b
k
(
?
b
k
g
δ
t
)
∧
(
a
b
k
+
1
?
b
k
+
1
a
)
δ
t
2
δ
b
k
g
f_{15}=\frac{1}{4} \frac{\delta R_{b_i b_k} (-b_k^g \delta t)^{\wedge}(a_b^{k+1} - b_{k+1}^a)\delta t^2}{\delta b_k^g}
f15?=41?δbkg?δRbi?bk??(?bkg?δt)∧(abk+1??bk+1a?)δt2?
使用伴随性质，有
f
15
=
1
4
δ
R
b
i
b
k
(
a
b
k
+
1
?
b
k
+
1
a
)
∧
(
b
k
g
δ
t
)
δ
t
2
δ
b
k
g
f_{15}=\frac{1}{4} \frac{\delta R_{b_i b_k} (a_b^{k+1} - b_{k+1}^a)^{\wedge}(b_k^g \delta t)\delta t^2}{\delta b_k^g}
f15?=41?δbkg?δRbi?bk??(abk+1??bk+1a?)∧(bkg?δt)δt2?
f
15
=
1
4
R
b
i
b
k
(
a
b
k
+
1
?
b
k
+
1
a
)
∧
δ
t
2
δ
t
f_{15}=\frac{1}{4} R_{b_i b_k} (a_b^{k+1} - b_{k+1}^a)^{\wedge} \delta t^2 \delta t
f15?=41?Rbi?bk??(abk+1??bk+1a?)∧δt2δt
g
12
=
δ
α
b
i
b
k
+
1
δ
n
k
g
g_{12}=\frac{\delta \alpha_{b_i b_{k+1}}}{\delta n_k^g}
g12?=δnkg?δαbi?bk+1???
一看
n
k
g
n_k^g
nkg?就知道又要找和旋转有关的量了。回顾递推公式，有
ω
=
1
2
(
(
ω
b
k
+
n
k
g
?
b
k
g
)
+
(
w
b
k
+
1
+
n
k
+
1
g
?
b
k
+
1
g
)
)
\omega = \frac{1}{2}((\omega_b^k+n_k^g-b_k^g)+(w_b^{k+1}+n_{k+1}^g-b_{k+1}^g))
ω=21?((ωbk?+nkg??bkg?)+(wbk+1?+nk+1g??bk+1g?))
q
b
i
b
k
+
1
=
q
b
i
b
k
?
[
1
,
1
2
ω
δ
t
]
T
q_{b_i b_{k+1}} = q_{b_i b_k} \otimes [1, \frac{1}{2} \omega \delta t]^T
qbi?bk+1??=qbi?bk???[1,21?ωδt]T
a
=
1
2
(
q
b
i
b
k
(
a
b
k
+
n
b
k
?
b
k
a
)
+
q
b
i
b
k
+
1
(
a
b
k
+
1
+
n
b
k
+
1
?
b
k
+
1
a
)
)
a = \frac{1}{2}(q_{b_i b_{k}} (a_b^k + n_b^k -b_k^a) + q_{b_i b_{k+1}} (a_b^{k+1} + n_b^{k+1} - b_{k+1}^a))
a=21?(qbi?bk??(abk?+nbk??bka?)+qbi?bk+1??(abk+1?+nbk+1??bk+1a?))
α
b
i
b
k
+
1
=
α
b
i
b
k
+
β
b
i
b
k
δ
t
+
1
2
a
δ
t
2
\alpha_{b_i b_{k+1}} = \alpha_{b_i b_{k}} + \beta_{b_i b_k} \delta t + \frac{1}{2}a \delta t^2
αbi?bk+1??=αbi?bk??+βbi?bk??δt+21?aδt2
有
g
12
=
δ
α
b
i
b
k
+
1
δ
n
k
g
g_{12}=\frac{\delta \alpha_{b_i b_{k+1}}}{\delta n_k^g}
g12?=δnkg?δαbi?bk+1???
g
12
=
δ
1
2
a
δ
t
2
δ
n
k
g
g_{12}=\frac{\delta \frac{1}{2}a \delta t^2}{\delta n_k^g}
g12?=δnkg?δ21?aδt2?
a
=
1
2
(
q
b
i
b
k
(
a
b
k
+
n
b
k
?
b
k
a
)
+
q
b
i
b
k
+
1
(
a
b
k
+
1
+
n
b
k
+
1
?
b
k
+
1
a
)
)
a = \frac{1}{2}(q_{b_i b_{k}} (a_b^k + n_b^k -b_k^a) + q_{b_i b_{k+1}} (a_b^{k+1} + n_b^{k+1} - b_{k+1}^a))
a=21?(qbi?bk??(abk?+nbk??bka?)+qbi?bk+1??(abk+1?+nbk+1??bk+1a?))
g
12
=
δ
1
4
q
b
i
b
k
+
1
(
a
b
k
+
1
?
b
k
+
1
a
)
δ
t
2
δ
n
k
g
g_{12}=\frac{\delta \frac{1}{4}q_{b_i b_{k+1}} (a_b^{k+1} - b_{k+1}^a) \delta t^2}{\delta n_k^g}
g12?=δnkg?δ41?qbi?bk+1??(abk+1??bk+1a?)δt2?
又因为
q
b
i
b
k
+
1
=
q
b
i
b
k
?
[
1
,
1
2
ω
δ
t
]
T
q_{b_i b_{k+1}} = q_{b_i b_k} \otimes [1, \frac{1}{2} \omega \delta t]^T
qbi?bk+1??=qbi?bk???[1,21?ωδt]T
所以有
g
12
=
δ
1
4
q
b
i
b
k
?
[
1
,
1
2
ω
δ
t
]
T
(
a
b
k
+
1
?
b
k
+
1
a
)
δ
t
2
δ
n
k
g
g_{12}=\frac{\delta \frac{1}{4}q_{b_i b_k} \otimes [1, \frac{1}{2} \omega \delta t]^T (a_b^{k+1} - b_{k+1}^a) \delta t^2}{\delta n_k^g}
g12?=δnkg?δ41?qbi?bk???[1,21?ωδt]T(abk+1??bk+1a?)δt2?
ω
=
1
2
(
(
ω
b
k
+
n
k
g
?
b
k
g
)
+
(
w
b
k
+
1
+
n
k
+
1
g
?
b
k
+
1
g
)
)
\omega = \frac{1}{2}((\omega_b^k+n_k^g-b_k^g)+(w_b^{k+1}+n_{k+1}^g-b_{k+1}^g))
ω=21?((ωbk?+nkg??bkg?)+(wbk+1?+nk+1g??bk+1g?))
g
12
=
δ
1
4
q
b
i
b
k
?
[
1
,
1
2
(
ω
b
k
+
1
2
n
k
g
)
δ
t
]
T
(
a
b
k
+
1
?
b
k
+
1
a
)
δ
t
2
δ
n
k
g
g_{12}=\frac{\delta \frac{1}{4}q_{b_i b_k} \otimes [1, \frac{1}{2} (\omega_b^k+\frac{1}{2}n_k^g)\delta t]^T (a_b^{k+1} - b_{k+1}^a) \delta t^2}{\delta n_k^g}
g12?=δnkg?δ41?qbi?bk???[1,21?(ωbk?+21?nkg?)δt]T(abk+1??bk+1a?)δt2?
g
12
=
1
4
δ
R
b
i
b
k
exp
?
(
(
(
ω
b
k
+
1
2
n
k
g
)
δ
t
)
∧
)
(
a
b
k
+
1
?
b
k
+
1
a
)
δ
t
2
δ
n
k
g
g_{12}=\frac{1}{4} \frac{\delta R_{b_i b_k} \exp(((\omega_b^k+\frac{1}{2}n_k^g)\delta t)^{\wedge})(a_b^{k+1} - b_{k+1}^a) \delta t^2}{\delta n_k^g}
g12?=41?δnkg?δRbi?bk??exp(((ωbk?+21?nkg?)δt)∧)(abk+1??bk+1a?)δt2?
g
12
=
1
4
δ
R
b
i
b
k
(
exp
?
(
(
ω
b
k
δ
t
)
∧
)
)
(
exp
?
(
(
J
r
(
ω
b
k
δ
t
)
1
2
n
k
g
δ
t
)
∧
)
)
(
a
b
k
+
1
?
b
k
+
1
a
)
δ
t
2
δ
n
k
g
g_{12}=\frac{1}{4} \frac{\delta R_{b_i b_k} (\exp((\omega_b^k\delta t)^{\wedge}))(\exp((J_r(\omega_b^k\delta t)\frac{1}{2}n_k^g \delta t)^{\wedge}))(a_b^{k+1} - b_{k+1}^a) \delta t^2}{\delta n_k^g}
g12?=41?δnkg?δRbi?bk??(exp((ωbk?δt)∧))(exp((Jr?(ωbk?δt)21?nkg?δt)∧))(abk+1??bk+1a?)δt2?
g
12
=
1
4
δ
R
b
i
b
k
(
exp
?
(
(
J
r
(
ω
b
k
δ
t
)
1
2
n
k
g
δ
t
)
∧
)
)
(
a
b
k
+
1
?
b
k
+
1
a
)
δ
t
2
δ
n
k
g
g_{12}=\frac{1}{4} \frac{\delta R_{b_i b_k}(\exp((J_r(\omega_b^k\delta t)\frac{1}{2}n_k^g \delta t)^{\wedge}))(a_b^{k+1} - b_{k+1}^a) \delta t^2}{\delta n_k^g}
g12?=41?δnkg?δRbi?bk??(exp((Jr?(ωbk?δt)21?nkg?δt)∧))(abk+1??bk+1a?)δt2?
g
12
=
1
4
δ
R
b
i
b
k
(
exp
?
(
(
1
2
n
k
g
δ
t
)
∧
)
)
(
a
b
k
+
1
?
b
k
+
1
a
)
δ
t
2
δ
n
k
g
g_{12}=\frac{1}{4} \frac{\delta R_{b_i b_k}(\exp((\frac{1}{2}n_k^g \delta t)^{\wedge}))(a_b^{k+1} - b_{k+1}^a) \delta t^2}{\delta n_k^g}
g12?=41?δnkg?δRbi?bk??(exp((21?nkg?δt)∧))(abk+1??bk+1a?)δt2?
g
12
=
1
4
δ
R
b
i
b
k
(
(
1
2
n
k
g
δ
t
)
∧
)
(
a
b
k
+
1
?
b
k
+
1
a
)
δ
t
2
δ
n
k
g
g_{12}=\frac{1}{4} \frac{\delta R_{b_i b_k}((\frac{1}{2}n_k^g \delta t)^{\wedge})(a_b^{k+1} - b_{k+1}^a) \delta t^2}{\delta n_k^g}
g12?=41?δnkg?δRbi?bk??((21?nkg?δt)∧)(abk+1??bk+1a?)δt2?
g
12
=
?
1
4
δ
R
b
i
b
k
(
a
b
k
+
1
?
b
k
+
1
a
)
∧
(
1
2
n
k
g
δ
t
)
δ
t
2
δ
n
k
g
g_{12}=-\frac{1}{4} \frac{\delta R_{b_i b_k}(a_b^{k+1} - b_{k+1}^a)^{\wedge} (\frac{1}{2}n_k^g \delta t)\delta t^2}{\delta n_k^g}
g12?=?41?δnkg?δRbi?bk??(abk+1??bk+1a?)∧(21?nkg?δt)δt2?
g
12
=
?
1
4
R
b
i
b
k
(
a
b
k
+
1
?
b
k
+
1
a
)
∧
(
1
2
δ
t
)
δ
t
2
g_{12}=-\frac{1}{4} R_{b_i b_k}(a_b^{k+1} - b_{k+1}^a)^{\wedge} (\frac{1}{2} \delta t)\delta t^2
g12?=?41?Rbi?bk??(abk+1??bk+1a?)∧(21?δt)δt2
g
12
=
?
1
8
R
b
i
b
k
(
a
b
k
+
1
?
b
k
+
1
a
)
∧
(
δ
t
)
δ
t
2
g_{12}=-\frac{1}{8} R_{b_i b_k}(a_b^{k+1} - b_{k+1}^a)^{\wedge} (\delta t)\delta t^2
g12?=?81?Rbi?bk??(abk+1??bk+1a?)∧(δt)δt2
Levenberg (1944) 和 Marquardt (1963) 先后对高斯牛顿法进行了改进，求解过程中引入了阻尼因子
(
J
T
J
+
μ
I
)
Δ
x
l
m
=
?
J
T
f
,
μ
>
0
(J^TJ+\mu I) \Delta x_{lm} = -J^Tf,\mu >0
(JTJ+μI)Δxlm?=?JTf,μ>0
F
(
x
)
=
1
2
∑
i
(
f
i
(
x
)
)
2
F(x)=\frac{1}{2} \sum_i (f_i(x))^2
F(x)=21?i∑?(fi?(x))2
J
=
δ
f
δ
x
J = \frac{\delta f}{\delta x}
J=δxδf?
F
′
(
x
)
=
(
J
T
f
)
T
F'(x)=(J^Tf)^T
F′(x)=(JTf)T
F
′
′
(
x
)
=
J
T
J
F''(x)=J^T J
F′′(x)=JTJ
J
T
J
=
V
T
Λ
V
J^TJ = V^T \Lambda V
JTJ=VTΛV
Δ
x
l
m
=
?
∑
j
=
1
n
v
j
T
F
′
T
λ
j
+
μ
v
j
\Delta x_{lm} = -\sum_{j=1}^n \frac{v_j^T F'^T}{\lambda_j + \mu} v_j
Δxlm?=?j=1∑n?λj?+μvjT?F′T?vj?
由原来的式子
(
J
T
J
+
μ
I
)
Δ
x
l
m
=
?
J
T
f
(J^TJ+\mu I) \Delta x_{lm} = -J^Tf
(JTJ+μI)Δxlm?=?JTf
进行代换有
(
V
T
Λ
V
+
μ
I
)
Δ
x
l
m
=
?
J
T
f
(V^T \Lambda V+\mu I) \Delta x_{lm} = -J^Tf
(VTΛV+μI)Δxlm?=?JTf
(
V
T
Λ
V
+
μ
V
T
V
)
Δ
x
l
m
=
?
J
T
f
(V^T \Lambda V+\mu V^T V) \Delta x_{lm} = -J^Tf
(VTΛV+μVTV)Δxlm?=?JTf
(
V
T
Λ
V
+
V
T
μ
V
)
Δ
x
l
m
=
?
J
T
f
(V^T \Lambda V+V^T\mu V) \Delta x_{lm} = -J^Tf
(VTΛV+VTμV)Δxlm?=?JTf
(
V
T
Λ
V
+
V
T
μ
I
V
)
Δ
x
l
m
=
?
J
T
f
(V^T \Lambda V+V^T\mu IV) \Delta x_{lm} = -J^Tf
(VTΛV+VTμIV)Δxlm?=?JTf
(
V
T
(
Λ
+
μ
I
)
V
)
Δ
x
l
m
=
?
J
T
f
(V^T (\Lambda + \mu I) V) \Delta x_{lm} = -J^Tf
(VT(Λ+μI)V)Δxlm?=?JTf
因为
F
′
(
x
)
=
(
J
T
f
)
T
F'(x)=(J^Tf)^T
F′(x)=(JTf)T
故有
(
V
T
(
Λ
+
μ
I
)
V
)
Δ
x
l
m
=
?
F
′
T
(V^T (\Lambda + \mu I) V) \Delta x_{lm} = -F'^T
(VT(Λ+μI)V)Δxlm?=?F′T
V
=
[
v
1
,
v
2
,
.
.
.
v
n
]
V = [v_1, v_2, ... v_n]
V=[v1?,v2?,...vn?]
其中
v
j
v_j
vj?为列向量
V
T
=
[
v
1
T
,
v
2
T
,
.
.
.
v
n
T
]
T
V^T = [v_1^T, v_2^T, ... v_n^T]^T
VT=[v1T?,v2T?,...vnT?]T
Λ
+
μ
I
=
[
λ
1
+
μ
λ
2
+
μ
λ
3
+
μ
.
.
.
λ
n
+
μ
]
\Lambda + \mu I = \begin{bmatrix} \lambda_1+\mu & & & & \\ & \lambda_2+\mu & & & \\ & & \lambda_3+\mu & & \\ & & & ... & \\ & & & & \lambda_n+\mu \end{bmatrix}
Λ+μI=
?λ1?+μ?λ2?+μ?λ3?+μ?...?λn?+μ?
?
记为
D
i
a
g
(
λ
i
+
μ
)
Diag(\lambda_i+\mu)
Diag(λi?+μ)
V
T
D
i
a
g
(
λ
i
+
μ
)
V
Δ
x
l
m
=
?
F
′
T
V^TDiag(\lambda_i+\mu)V \Delta x_{lm} = -F'^T
VTDiag(λi?+μ)VΔxlm?=?F′T
得证
Δ
x
l
m
=
?
∑
j
=
1
n
v
j
T
F
′
T
λ
j
+
μ
v
j
\Delta x_{lm} = -\sum_{j=1}^n \frac{v_j^T F'^T}{\lambda_j + \mu} v_j
Δxlm?=?j=1∑n?λj?+μvjT?F′T?vj?