Fast-LIO2中IEKF前向推导过程记录
Fast-LIO2中使用了迭代误差状态卡尔曼滤波进行数据融合，算法实现位于IKFoM中，基于流形实现，论文中的推导过程较为晦涩，因此本文在这里基于自己的理解对前向推导过程进行一下梳理。
1 误差状态运动方程
(公式中下标k表示第k时刻，
x
^
k
\hat x_k
x^k 表示第k时刻的先验值
δ
x
k
\delta x_k
δxk 表示第k时刻的误差
u
k
u_k
uk 表示第k时刻的输入
w
k
w_k
wk 表示第k时刻的运动噪声)
首先引出误差状态的递归公式
δ
x
k
+
1
=
(
(
x
^
k
⊞
δ
x
k
)
⊕
(
Δ
t
f
(
x
^
k
⊞
δ
x
k
,
u
k
,
w
k
)
)
)
⊟
(
x
^
k
⊕
(
Δ
t
f
(
x
^
k
,
u
k
,
w
k
)
)
)
,
真值状态变量减去名义状态变量
\delta x_{k+1}=((\hat x_k \boxplus \textcolor{red}{\delta x_k}) \oplus (\Delta t f(\hat x_k \boxplus \textcolor{red}{\delta x_k}, u_k, w_k)))\\ \boxminus (\hat x_k \oplus (\Delta t f(\hat x_k, u_k, w_k))),\\[10pt]真值状态变量减去名义状态变量
δxk+1=((x^k⊞δxk)⊕(Δtf(x^k⊞δxk,uk,wk)))⊟(x^k⊕(Δtf(x^k,uk,wk))),真值状态变量减去名义状态变量
对上式线性化得到
δ
x
k
+
1
=
F
x
k
δ
x
k
+
F
ω
k
ω
k
\delta x_{k+1}=F_{x_k} \delta x_k+F_{\omega_k}\omega_k
δxk+1=Fxkδxk+Fωkωk
其中
F
x
k
F_{x_k}
Fxk是递归公式对
δ
x
k
\textcolor{red}{\delta x_k}
δxk求偏导：
F
X
k
=
∂
(
(
x
^
k
⊞
δ
x
k
)
⊕
(
Δ
t
f
(
x
^
k
⊞
δ
x
k
,
u
k
,
0
)
)
)
⊟
(
x
^
k
⊕
(
Δ
t
f
(
x
^
k
,
u
k
,
0
)
)
)
∂
δ
x
k
=
∂
(
(
x
^
k
⊞
δ
x
k
)
⊕
(
Δ
t
f
(
x
^
k
,
u
k
,
0
)
)
)
⊟
(
x
^
k
⊕
(
Δ
t
f
(
x
^
k
,
u
k
,
0
)
)
)
∂
δ
x
k
+
∂
(
(
x
^
k
)
⊕
(
Δ
t
f
(
x
^
k
⊞
δ
x
k
,
u
k
,
0
)
)
)
⊟
(
x
^
k
⊕
(
Δ
t
f
(
x
^
k
,
u
k
,
0
)
)
)
∂
δ
x
k
=
∂
(
(
x
^
k
⊞
δ
x
k
)
⊕
(
Δ
t
f
(
x
^
k
,
u
k
,
0
)
)
)
⊟
(
x
^
k
⊕
(
Δ
t
f
(
x
^
k
,
u
k
,
0
)
)
)
∂
δ
x
k
+
∂
(
(
x
^
k
)
⊕
(
Δ
t
f
(
x
^
k
⊞
δ
x
k
,
u
k
,
0
)
)
)
⊟
(
x
^
k
⊕
(
Δ
t
f
(
x
^
k
,
u
k
,
0
)
)
)
∂
Δ
t
f
(
x
^
k
⊞
δ
x
k
,
u
k
,
0
)
∂
Δ
t
f
(
x
^
k
⊞
δ
x
k
,
u
k
,
0
)
∂
δ
x
k
=
G
x
k
+
Δ
t
G
f
k
∂
f
(
x
^
k
⊞
δ
x
k
,
u
k
,
0
)
∂
δ
x
k
∣
δ
x
k
=
0
F_{X_k}= \frac{\partial ((\hat x_k \boxplus \textcolor{red}{\delta x_k}) \oplus (\Delta t f(\hat x_k \boxplus \textcolor{red}{\delta x_k}, u_k, 0)))\\ \boxminus (\hat x_k \oplus (\Delta t f(\hat x_k, u_k, 0)))}{\partial \delta x_k}\\[20pt] =\frac{\partial ((\hat x_k \boxplus \textcolor{red}{\delta x_k}) \oplus (\Delta t f(\hat x_k, u_k, 0)))\\ \boxminus (\hat x_k \oplus (\Delta t f(\hat x_k, u_k, 0)))}{\partial \delta x_k}\\[5pt] +\frac{\partial ((\hat x_k) \oplus (\Delta t f(\hat x_k \boxplus \textcolor{red}{\delta x_k}, u_k, 0)))\\ \boxminus (\hat x_k \oplus (\Delta t f(\hat x_k, u_k, 0)))}{\partial \delta x_k}\\[20pt] =\frac{\partial ((\hat x_k \boxplus \textcolor{red}{\delta x_k}) \oplus (\Delta t f(\hat x_k, u_k, 0)))\\ \boxminus (\hat x_k \oplus (\Delta t f(\hat x_k, u_k, 0)))}{\partial \delta x_k}\\[5pt] +\frac{\partial ((\hat x_k) \oplus (\Delta t f(\hat x_k \boxplus \textcolor{red}{\delta x_k}, u_k, 0)))\\ \boxminus (\hat x_k \oplus (\Delta t f(\hat x_k, u_k, 0)))}{\partial \Delta t f(\hat x_k \boxplus \textcolor{red}{\delta x_k}, u_k, 0)} \frac{\partial \Delta t f(\hat{x}_k \boxplus \textcolor{red}{\delta x_k},u_k,0)}{\partial \delta x_k}\\[20pt] =G_{x_k}+\Delta t G_{f_k} \frac{\partial f(\hat{x}_k \boxplus \textcolor{red}{\delta x_k},u_k,0)}{\partial \delta x_k}|_{ \delta x_k = 0}\\
FXk=∂δxk∂((x^k⊞δxk)⊕(Δtf(x^k⊞δxk,uk,0)))⊟(x^k⊕(Δtf(x^k,uk,0)))=∂δxk∂((x^k⊞δxk)⊕(Δtf(x^k,uk,0)))⊟(x^k⊕(Δtf(x^k,uk,0)))+∂δxk∂((x^k)⊕(Δtf(x^k⊞δxk,uk,0)))⊟(x^k⊕(Δtf(x^k,uk,0)))=∂δxk∂((x^k⊞δxk)⊕(Δtf(x^k,uk,0)))⊟(x^k⊕(Δtf(x^k,uk,0)))+∂Δtf(x^k⊞δxk,uk,0)∂((x^k)⊕(Δtf(x^k⊞δxk,uk,0)))⊟(x^k⊕(Δtf(x^k,uk,0)))∂δxk∂Δtf(x^k⊞δxk,uk,0)=Gxk+ΔtGfk∂δxk∂f(x^k⊞δxk,uk,0)∣δxk=0
其中令：
G
x
k
=
∂
(
(
x
^
k
⊞
δ
x
k
)
⊕
(
Δ
t
f
(
x
^
k
,
u
k
,
0
)
)
)
⊟
(
x
^
k
⊕
(
Δ
t
f
(
x
^
k
,
u
k
,
0
)
)
)
∂
δ
x
k
=
∂
(
(
(
x
⊞
u
)
⊕
v
)
⊟
y
)
∂
u
∣
x
=
x
^
k
;
u
=
δ
x
k
=
0
;
v
=
Δ
t
f
(
x
^
k
,
u
k
,
0
)
;
y
=
x
^
k
⊕
Δ
t
f
(
x
^
k
,
u
k
,
0
)
G
f
k
=
∂
(
(
x
^
k
)
⊕
(
Δ
t
f
(
x
^
k
⊞
δ
x
k
,
u
k
,
0
)
)
)
⊟
(
x
^
k
⊕
(
Δ
t
f
(
x
^
k
,
u
k
,
0
)
)
)
∂
Δ
t
f
(
x
^
k
⊞
δ
x
k
,
u
k
,
0
)
=
∂
(
(
(
x
⊞
u
)
⊕
v
)
⊟
y
)
∂
v
∣
x
=
x
^
k
;
u
=
0
;
v
=
Δ
t
f
(
x
^
k
,
u
k
,
0
)
;
y
=
x
^
k
⊕
Δ
t
f
(
x
^
k
,
u
k
,
0
)
G_{x_k}=\frac{\partial ((\hat x_k \boxplus \textcolor{red}{\delta x_k}) \oplus (\Delta t f(\hat x_k, u_k, 0)))\\ \boxminus (\hat x_k \oplus (\Delta t f(\hat x_k, u_k, 0)))}{\partial \delta x_k}\\[20pt] =\frac{\partial(((x \boxplus u)\oplus v)\boxminus y)}{\partial u}|_{x=\hat x_k;u= \textcolor{red}{\delta x_k}=0;v=\Delta t f(\hat x_k, u_k, 0);y=\hat x_k \oplus \Delta tf(\hat x_k,u_k,0)}\\[30pt] G_{f_k}=\frac{\partial ((\hat x_k) \oplus (\Delta t f(\hat x_k \boxplus \textcolor{red}{\delta x_k}, u_k, 0)))\\ \boxminus (\hat x_k \oplus (\Delta t f(\hat x_k, u_k, 0)))}{\partial \Delta t f(\hat x_k \boxplus \textcolor{red}{\delta x_k}, u_k, 0)} \\[20pt] =\frac{\partial(((x \boxplus u)\oplus v)\boxminus y)}{\partial v}|_{x=\hat x_k;\textcolor{red}{u=0};v=\Delta t f(\hat x_k, u_k, 0);y=\hat x_k \oplus \Delta tf(\hat x_k,u_k,0)}
Gxk=∂δxk∂((x^k⊞δxk)⊕(Δtf(x^k,uk,0)))⊟(x^k⊕(Δtf(x^k,uk,0)))=∂u∂(((x⊞u)⊕v)⊟y)∣x=x^k;u=δxk=0;v=Δtf(x^k,uk,0);y=x^k⊕Δtf(x^k,uk,0)Gfk=∂Δtf(x^k⊞δxk,uk,0)∂((x^k)⊕(Δtf(x^k⊞δxk,uk,0)))⊟(x^k⊕(Δtf(x^k,uk,0)))=∂v∂(((x⊞u)⊕v)⊟y)∣x=x^k;u=0;v=Δtf(x^k,uk,0);y=x^k⊕Δtf(x^k,uk,0)
则：
F
X
k
=
G
x
k
+
Δ
t
G
f
k
∂
f
(
x
^
k
⊞
δ
x
k
,
u
k
,
0
)
∂
δ
x
k
∣
δ
x
=
0
F
W
k
=
Δ
t
G
f
k
∂
f
(
x
^
k
,
u
k
,
ω
)
∂
ω
∣
ω
=
0
F_{X_k}=G_{x_k}+\Delta t G_{f_k} \frac{\partial f(\hat{x}_k \boxplus \textcolor{red}{\delta x_k},u_k,0)}{\partial \delta x_k}|_{ \delta x = 0}\\ F_{W_k}=\Delta t G_{f_k} \frac{\partial f(\hat{x}_k ,u_k,\omega)}{\partial \omega}|_{ \omega = 0}
FXk=Gxk+ΔtGfk∂δxk∂f(x^k⊞δxk,uk,0)∣δx=0FWk=ΔtGfk∂ω∂f(x^k,uk,ω)∣ω=0
2 各分量推导
（1）离散模型中增量部分对 δ x \delta x δx求偏导
首先推导
∂
f
(
x
^
k
⊞
δ
x
,
u
k
,
0
)
∂
δ
x
∣
δ
x
=
0
\frac{\partial f(\hat{x}_k \boxplus \delta x,u_k,0)}{\partial \delta x}|_{ \delta x = 0}
∂δx∂f(x^k⊞δx,uk,0)∣δx=0
其中根据论文中的定义
f
(
x
^
k
⊞
δ
x
,
u
k
,
0
)
=
[
v
^
k
+
δ
v
ω
k
−
b
^
g
−
δ
b
g
0
3
×
3
0
3
×
3
R
^
k
E
x
p
(
δ
θ
)
(
a
k
−
b
^
a
−
δ
b
a
)
+
g
0
3
×
3
0
3
×
3
0
3
×
3
]
18
×
1
f(\hat{x}_k \boxplus \delta x,u_k,0)= \begin{bmatrix} \hat v_k+\delta v \\ \omega_k-\hat b_g-\delta b_g \\ 0_{3\times 3} \\ 0_{3\times 3} \\ \hat R_kExp(\delta \theta)(a_k-\hat b_a-\delta b_a)+g \\ 0_{3\times 3}\\ 0_{3\times 3}\\ 0_{3\times 3} \end{bmatrix}_{18 \times1}
f(x^k⊞δx,uk,0)=
v^k+δvωk−b^g−δbg03×303×3R^kExp(δθ)(ak−b^a−δba)+g03×303×303×3
18×1
δ
x
=
[
δ
p
δ
θ
δ
θ
L
I
δ
p
L
I
δ
v
δ
b
g
δ
b
a
δ
g
]
T
\delta x= \begin{bmatrix} \delta p& \delta \theta& \delta \theta^I_L& \delta p^I_L& \delta v& \delta b_g& \delta b_a& \delta g \end{bmatrix}^T
δx=[δpδθδθLIδpLIδvδbgδbaδg]T
因此求导可得如下稀疏矩阵：
∂
f
(
x
^
k
⊞
δ
x
,
u
k
,
0
)
∂
δ
x
∣
δ
x
=
0
=
[
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
I
3
×
3
0
3
×
3
0
3
×
3
0
3
×
2
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
−
I
3
×
3
0
3
×
3
0
3
×
2
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
2
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
2
0
3
×
3
−
R
^
k
(
a
k
−
b
^
a
)
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
−
R
^
k
∂
g
∂
δ
g
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
2
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
2
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
2
]
\frac{\partial f(\hat{x}_k \boxplus \delta x,u_k,0)}{\partial \delta x}|_{ \delta x = 0}= \begin{bmatrix} 0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&I_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 2}\\ 0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&-I_{3\times 3}&0_{3\times 3}&0_{3\times 2}\\ 0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 2}\\ 0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 2}\\ 0_{3\times 3}&-\hat{R}_k(a_k-\hat b_a)&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&-\hat{R}_k&\frac{\partial g}{\partial \delta g}\\ 0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 2}\\ 0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 2}\\ 0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 2}\\ \end{bmatrix}
∂δx∂f(x^k⊞δx,uk,0)∣δx=0=
03×303×303×303×303×303×303×303×303×303×303×303×3−R^k(ak−b^a)03×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×3I3×303×303×303×303×303×303×303×303×3−I3×303×303×303×303×303×303×303×303×303×303×3−R^k03×303×303×303×203×203×203×2∂δg∂g03×203×203×2
（2） 离散模型中增量部分对 w w w求偏导
然后推导
∂
f
(
x
^
k
,
u
k
,
ω
)
∂
ω
∣
ω
=
0
\frac{\partial f(\hat{x}_k ,u_k,\omega)}{\partial \omega}|_{ \omega = 0}
∂ω∂f(x^k,uk,ω)∣ω=0
其中根据论文中的定义
f
(
x
^
k
,
u
k
,
ω
)
=
[
v
^
k
ω
k
−
b
^
g
−
n
ω
0
3
×
1
0
3
×
1
R
^
k
(
a
k
−
b
^
a
−
n
a
)
+
g
n
b
g
n
b
a
0
3
×
1
]
18
×
1
f(\hat{x}_k ,u_k,\omega)= \begin{bmatrix} \hat v_k\\ \omega_k-\hat b_g-n_{\omega} \\ 0_{3\times 1} \\ 0_{3\times 1} \\ \hat R_k(a_k-\hat b_a-n_a)+g \\ n_{b_g}\\ n_{b_a}\\ 0_{3\times 1} \end{bmatrix}_{18 \times1}
f(x^k,uk,ω)=
v^kωk−b^g−nω03×103×1R^k(ak−b^a−na)+gnbgnba03×1
18×1
ω
=
[
n
ω
n
a
n
b
g
n
b
a
]
T
\omega=\begin{bmatrix} n_{\omega}& n_a&n_{b_g}& n_{b_a}\end{bmatrix}^T
ω=[nωnanbgnba]T
因此求导可得如下稀疏矩阵：
∂
f
(
x
^
k
⊞
δ
x
,
u
k
,
0
)
∂
δ
x
∣
δ
x
=
0
=
[
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
−
I
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
−
R
^
k
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
I
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
I
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
]
\frac{\partial f(\hat{x}_k \boxplus \delta x,u_k,0)}{\partial \delta x}|_{ \delta x = 0}= \begin{bmatrix} 0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}\\ -I_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}\\ 0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}\\ 0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}\\ 0_{3\times 3}&-\hat{R}_k&0_{3\times 3}&0_{3\times 3}\\ 0_{3\times 3}&0_{3\times 3}&I_{3\times 3}&0_{3\times 3}\\ 0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&I_{3\times 3}\\ 0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}\\ \end{bmatrix}
∂δx∂f(x^k⊞δx,uk,0)∣δx=0=
03×3−I3×303×303×303×303×303×303×303×303×303×303×3−R^k03×303×303×303×303×303×303×303×3I3×303×303×303×303×303×303×303×303×3I3×303×3
（3） G x k G_{x_k} Gxk推导
G
x
k
=
∂
(
(
x
^
k
⊞
δ
x
k
)
⊕
(
Δ
t
f
(
x
^
k
,
u
k
,
0
)
)
)
⊟
(
x
^
k
⊕
(
Δ
t
f
(
x
^
k
,
u
k
,
0
)
)
)
∂
δ
x
k
=
∂
(
(
(
x
⊞
u
)
⊕
v
)
⊟
y
)
∂
u
∣
x
=
x
^
k
;
u
=
δ
x
k
=
0
;
v
=
Δ
t
f
(
x
^
k
,
u
k
,
0
)
;
y
=
x
^
k
⊕
Δ
t
f
(
x
^
k
,
u
k
,
0
)
=
SO3
A
(
(
(
x
⊞
u
)
⊕
v
)
⊟
y
)
−
T
E
x
p
(
−
v
)
A
(
u
)
−
T
=
S2
N
(
(
x
⊞
u
)
⊕
v
,
y
)
R
(
v
)
M
(
x
,
u
)
G_{x_k}=\frac{\partial ((\hat x_k \boxplus \textcolor{red}{\delta x_k}) \oplus (\Delta t f(\hat x_k, u_k, 0)))\\ \boxminus (\hat x_k \oplus (\Delta t f(\hat x_k, u_k, 0)))}{\partial \delta x_k}\\[20pt] =\frac{\partial(((x \boxplus u)\oplus v)\boxminus y)}{\partial u}|_{x=\hat x_k;u= \textcolor{red}{\delta x_k}=0;v=\Delta t f(\hat x_k, u_k, 0);y=\hat x_k \oplus \Delta tf(\hat x_k,u_k,0)}\\[20pt] \stackrel{\text{SO3}}{=} A(((x \boxplus u)\oplus v)\boxminus y)^{-T}Exp(-v)A(u)^{-T}\\[20pt] \stackrel{\text{S2}}{=} N((x \boxplus u)\oplus v, y)R(v)M(x,u)
Gxk=∂δxk∂((x^k⊞δxk)⊕(Δtf(x^k,uk,0)))⊟(x^k⊕(Δtf(x^k,uk,0)))=∂u∂(((x⊞u)⊕v)⊟y)∣x=x^k;u=δxk=0;v=Δtf(x^k,uk,0);y=x^k⊕Δtf(x^k,uk,0)=SO3A(((x⊞u)⊕v)⊟y)−TExp(−v)A(u)−T=S2N((x⊞u)⊕v,y)R(v)M(x,u)
TODO:更详细的推导
（4） G f k G_{f_k} Gfk推导
G
f
k
=
∂
(
(
x
^
k
)
⊕
(
Δ
t
f
(
x
^
k
⊞
δ
x
k
,
u
k
,
0
)
)
)
⊟
(
x
^
k
⊕
(
Δ
t
f
(
x
^
k
,
u
k
,
0
)
)
)
∂
Δ
t
f
(
x
^
k
⊞
δ
x
k
,
u
k
,
0
)
=
∂
(
(
(
x
⊞
u
)
⊕
v
)
⊟
y
)
∂
v
∣
x
=
x
^
k
;
u
=
0
;
v
=
Δ
t
f
(
x
^
k
,
u
k
,
0
)
;
y
=
x
^
k
⊕
Δ
t
f
(
x
^
k
,
u
k
,
0
)
=
SO3
A
(
(
(
x
⊞
u
)
⊕
v
)
⊟
y
)
−
T
A
(
v
)
−
T
=
S2
−
N
(
(
x
⊞
u
)
⊕
v
,
y
)
R
(
v
)
(
x
⊞
u
)
∧
A
(
v
)
T
G_{f_k}=\frac{\partial ((\hat x_k) \oplus (\Delta t f(\hat x_k \boxplus \textcolor{red}{\delta x_k}, u_k, 0)))\\ \boxminus (\hat x_k \oplus (\Delta t f(\hat x_k, u_k, 0)))}{\partial \Delta t f(\hat x_k \boxplus \textcolor{red}{\delta x_k}, u_k, 0)} \\[20pt] =\frac{\partial(((x \boxplus u)\oplus v)\boxminus y)}{\partial v}|_{x=\hat x_k;\textcolor{red}{u=0};v=\Delta t f(\hat x_k, u_k, 0);y=\hat x_k \oplus \Delta tf(\hat x_k,u_k,0)}\\[20pt] \stackrel{\text{SO3}}{=} A(((x \boxplus u)\oplus v)\boxminus y)^{-T} A(v)^{-T}\\[20pt] \stackrel{\text{S2}}{=} -N((x \boxplus u)\oplus v, y)R(v)(x \boxplus u)^{\wedge}A(v)^T
Gfk=∂Δtf(x^k⊞δxk,uk,0)∂((x^k)⊕(Δtf(x^k⊞δxk,uk,0)))⊟(x^k⊕(Δtf(x^k,uk,0)))=∂v∂(((x⊞u)⊕v)⊟y)∣x=x^k;u=0;v=Δtf(x^k,uk,0);y=x^k⊕Δtf(x^k,uk,0)=SO3A(((x⊞u)⊕v)⊟y)−TA(v)−T=S2−N((x⊞u)⊕v,y)R(v)(x⊞u)∧A(v)T
TODO:更详细的推导
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
