根据3.3的说法，也即是公式（14），相机的pose是IMU的pose而来。相机和IMU之间的外参是固定的，所以这两者知道一个就知道另一个了。
所谓的状态扩增就是增加一个相机的位姿，新增加的相机位姿啥都不知道？
要根据IMU的位姿来计算（因为IMU是实时更新的？）。
q
=
q
⊗
q
q=q\otimes q
q=q⊗q
p
=
q
p
+
p
p = qp+p
p=qp+p
所谓的协方差按照狄拉克记号就是
P
=
∣
X
>
<
X
∣
P=|X><X|
P=∣X><X∣,当|X>变成|X,x>的时候，P就变成了|X,x><X,x|, 关键就是|x><X|尚且未知，x=X
按照文章的意思应该有：
|X,x>=J|X>=
(
I
15
+
6
N
J
6
×
(
15
+
6
N
)
)
(
21
+
6
N
)
×
(
15
+
6
N
)
∣
X
15
+
6
N
>
=
(
X
J
6
×
(
15
+
6
N
)
X
)
\begin{pmatrix}I_{15+6N}\\J_{6\times (15+6N)}\end{pmatrix}_{(21+6N)\times (15+6N)}|X_{15+6N}>=\begin{pmatrix}X\\J_{6\times (15+6N)}X\end{pmatrix}
(I15+6NJ6×(15+6N))(21+6N)×(15+6N)∣X15+6N>=(XJ6×(15+6N)X)
继而应该有：
(
θ
N
+
1
p
N
+
1
)
=
J
6
×
(
15
+
6
N
)
X
\begin{pmatrix} \theta_{N+1}\\p_{N+1}\end{pmatrix}=J_{6\times (15+6N)}X
(θN+1pN+1)=J6×(15+6N)X
X I M U = ( q , b g , v , b a , P ) X_{IMU} = (q, b_g, v, b_a, P) XIMU=(q,bg,v,ba,P)
J 6 × ( 15 + 6 N ) = ( ∂ θ ∂ X ∂ P ∂ X ) = ( ∂ θ ∂ X I M U , 0 3 × 6 N ∂ P ∂ X I M U , 0 3 × 6 N ) = ( ∂ θ ∂ q , 0 3 × 9 , ∂ θ ∂ P , 0 3 × 6 N ∂ P ∂ q , 0 3 × 9 , ∂ P ∂ P , 0 3 × 6 N ) = ( q , 0 3 × 9 , 0 3 × 3 , 0 3 × 6 N q p × , 0 3 × 9 , I 3 × 3 , 0 3 × 6 N ) J_{6\times (15+6N)}=\begin{pmatrix}\frac{\partial \theta}{\partial X}\\\frac{\partial P}{\partial X}\end{pmatrix}=\begin{pmatrix}\frac{\partial \theta}{\partial X_{IMU}},0_{3\times 6N}\\ \frac{\partial P}{\partial X_{IMU}},0_{3\times 6N}\end{pmatrix}=\begin{pmatrix}\frac{\partial \theta}{\partial q},0_{3\times 9},\frac{\partial \theta}{\partial P},0_{3\times 6N}\\ \frac{\partial P}{\partial q},0_{3\times 9},\frac{\partial P}{\partial P},0_{3\times 6N}\end{pmatrix}=\begin{pmatrix}q,0_{3\times 9},0_{3\times3},0_{3\times 6N}\\ qp\times,0_{3\times 9},I_{3\times3},0_{3\times 6N}\end{pmatrix} J6×(15+6N)=(∂X∂θ∂X∂P)=(∂XIMU∂θ,03×6N∂XIMU∂P,03×6N)=(∂q∂θ,03×9,∂P∂θ,03×6N∂q∂P,03×9,∂P∂P,03×6N)=(q,03×9,03×3,03×6Nqp×,03×9,I3×3,03×6N)
4020
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
