1、姿态误差
ϕ
˙
=
M
a
a
ϕ
+
M
a
v
δ
v
n
+
M
a
p
δ
p
−
ω
i
b
x
b
C
b
n
δ
K
G
x
−
ω
i
b
y
b
C
b
n
δ
K
G
y
−
ω
i
b
z
b
C
b
n
δ
K
G
z
−
C
n
b
ε
b
\dot \phi = M_{aa}\phi + M_{av}\delta v^n + M_{ap}\delta p - \omega_{ibx}^{b} C_b^n \delta K_{Gx} - \omega_{iby}^{b} C_b^n \delta K_{Gy} - \omega_{ibz}^{b} C_b^n \delta K_{Gz} - C_n^b \varepsilon^b
ϕ˙=Maaϕ+Mavδvn+Mapδp−ωibxbCbnδKGx−ωibybCbnδKGy−ωibzbCbnδKGz−Cnbεb
=
[
M
a
a
M
a
v
M
a
p
−
C
b
n
O
3
×
3
]
[
ϕ
T
(
δ
v
n
)
T
(
δ
p
n
)
T
(
ε
b
)
T
(
∇
b
)
T
]
−
ω
i
b
x
b
C
b
n
δ
K
G
x
−
ω
i
b
y
b
C
b
n
δ
K
G
y
−
ω
i
b
z
b
C
b
n
δ
K
G
z
= \begin{bmatrix} M_{aa} & M_{av} & M_{ap} & -C_b^n & O_{3×3} \end{bmatrix} \begin{bmatrix} \phi^T \\ \\ (\delta v^n)^T \\ \\ (\delta p^n)^T \\ \\ (\varepsilon^b)^T \\ \\ (\nabla^b)^T \\ \end{bmatrix} - \omega_{ibx}^{b} C_b^n \delta K_{Gx} - \omega_{iby}^{b} C_b^n \delta K_{Gy} - \omega_{ibz}^{b} C_b^n \delta K_{Gz}
=[MaaMavMap−CbnO3×3]⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡ϕT(δvn)T(δpn)T(εb)T(∇b)T⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤−ωibxbCbnδKGx−ωibybCbnδKGy−ωibzbCbnδKGz
=
[
M
a
a
M
a
v
M
a
p
−
C
b
n
O
3
×
3
]
[
ϕ
T
(
δ
v
n
)
T
(
δ
p
n
)
T
(
ε
b
)
T
(
∇
b
)
T
]
−
C
b
n
W
g
b
= \begin{bmatrix} M_{aa} & M_{av} & M_{ap} & -C_b^n & O_{3×3} \end{bmatrix} \begin{bmatrix} \phi^T \\ \\ (\delta v^n)^T \\ \\ (\delta p^n)^T \\ \\ (\varepsilon^b)^T \\ \\ (\nabla^b)^T \\ \end{bmatrix} - C_b^n W_g^b
=[MaaMavMap−CbnO3×3]⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡ϕT(δvn)T(δpn)T(εb)T(∇b)T⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤−CbnWgb
其中：
M
a
a
=
−
(
ω
i
n
n
×
)
,
M
a
v
=
[
0
−
1
/
R
M
h
0
1
/
R
N
h
0
0
t
a
n
L
/
R
N
h
0
0
]
,
M
a
p
=
M
1
+
M
2
M_{aa}=-(\omega_{in}^n×),\space\space M_{av}= \begin{bmatrix} 0 & -1/R_{Mh} & 0 \\ \\ 1/R_{Nh} & 0 & 0 \\ \\ tanL/R_{Nh} & 0 & 0\\ \end{bmatrix} ,\space\space M_{ap}=M_1+M_2
Maa=−(ωinn×), Mav=⎣⎢⎢⎢⎢⎡01/RNhtanL/RNh−1/RMh00000⎦⎥⎥⎥⎥⎤, Map=M1+M2
M
1
=
[
0
0
0
−
ω
i
e
s
i
n
L
0
0
ω
i
e
c
o
s
L
0
0
]
,
M
2
=
[
0
0
v
N
R
M
h
2
0
0
−
v
E
/
R
N
h
2
v
E
s
e
c
2
L
/
R
N
h
0
−
v
E
t
a
n
L
/
R
N
h
2
]
M_1 = \begin{bmatrix} 0 & 0 & 0 \\ \\ -\omega_{ie}sinL & 0 & 0 \\ \\ \omega_{ie}cosL & 0 & 0 \\ \end{bmatrix} , \space\space M_2 = \begin{bmatrix} 0 & 0 & v_N R_{Mh}^2 \\ \\ 0 & 0 & -v_E/R_{Nh}^2 \\ \\ v_Esec^2L/R_{Nh} & 0 & -v_E tanL/R_{Nh}^2 \\ \end{bmatrix}
M1=⎣⎢⎢⎢⎢⎡0−ωiesinLωiecosL000000⎦⎥⎥⎥⎥⎤, M2=⎣⎢⎢⎢⎢⎡00vEsec2L/RNh000vNRMh2−vE/RNh2−vEtanL/RNh2⎦⎥⎥⎥⎥⎤
W
g
b
=
ω
i
b
x
b
δ
K
G
x
+
ω
i
b
y
b
δ
K
G
y
+
ω
i
b
z
b
δ
K
G
z
W_g^b = \omega_{ibx}^{b} \delta K_{Gx} + \omega_{iby}^{b} \delta K_{Gy} + \omega_{ibz}^{b} \delta K_{Gz}
Wgb=ωibxbδKGx+ωibybδKGy+ωibzbδKGz
2、速度误差
δ v ˙ n = M v a ϕ + M v v δ v n + M v p δ p + f s f x b C b n δ K A x + f s f y b C b n δ K A y + f s f z b C b n δ K A z + C n b ∇ b \delta \dot v^n = M_{va}\phi + M_{vv}\delta v^n + M_{vp}\delta p + f_{sfx}^{b} C_b^n \delta K_{Ax} + f_{sfy}^{b} C_b^n \delta K_{Ay} + f_{sfz}^{b} C_b^n \delta K_{Az} + C_n^b \nabla^b δv˙n=Mvaϕ+Mvvδvn+Mvpδp+fsfxbCbnδKAx+fsfybCbnδKAy+fsfzbCbnδKAz+Cnb∇b
=
[
M
v
a
M
v
v
M
v
p
O
3
×
3
C
b
n
]
[
ϕ
T
(
δ
v
n
)
T
(
δ
p
n
)
T
(
ε
b
)
T
(
∇
b
)
T
]
+
f
s
f
x
b
C
b
n
δ
K
A
x
+
f
s
f
y
b
C
b
n
δ
K
A
y
+
f
s
f
z
b
C
b
n
δ
K
A
z
= \begin{bmatrix} M_{va} & M_{vv} & M_{vp} & O_{3×3} & C_b^n \end{bmatrix} \begin{bmatrix} \phi^T \\ \\ (\delta v^n)^T \\ \\ (\delta p^n)^T \\ \\ (\varepsilon^b)^T \\ \\ (\nabla^b)^T \\ \end{bmatrix} + f_{sfx}^{b} C_b^n \delta K_{Ax} + f_{sfy}^{b} C_b^n \delta K_{Ay} + f_{sfz}^{b} C_b^n \delta K_{Az}
=[MvaMvvMvpO3×3Cbn]⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡ϕT(δvn)T(δpn)T(εb)T(∇b)T⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤+fsfxbCbnδKAx+fsfybCbnδKAy+fsfzbCbnδKAz
=
[
M
v
a
M
v
v
M
v
p
O
3
×
3
C
b
n
]
[
ϕ
T
(
δ
v
n
)
T
(
δ
p
n
)
T
(
ε
b
)
T
(
∇
b
)
T
]
+
C
b
n
W
a
b
= \begin{bmatrix} M_{va} & M_{vv} & M_{vp} & O_{3×3} & C_b^n \end{bmatrix} \begin{bmatrix} \phi^T \\ \\ (\delta v^n)^T \\ \\ (\delta p^n)^T \\ \\ (\varepsilon^b)^T \\ \\ (\nabla^b)^T \\ \end{bmatrix} + C_b^n W_a^b
=[MvaMvvMvpO3×3Cbn]⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡ϕT(δvn)T(δpn)T(εb)T(∇b)T⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤+CbnWab
其中：
M
v
a
=
(
f
s
f
n
×
)
M_{va} = (f_{sf}^n×)
Mva=(fsfn×)
M
v
v
=
(
v
n
×
)
M
a
v
−
(
(
2
ω
i
e
n
+
ω
e
n
n
)
×
)
M_{vv} = (v^n×)M_{av} - ((2\omega_{ie}^n+\omega_{en}^n)×)
Mvv=(vn×)Mav−((2ωien+ωenn)×)
M
v
p
=
(
v
n
×
)
(
2
M
1
+
M
2
)
+
M
3
M_{vp} = (v^n×)(2M_1+M_2)+M_3
Mvp=(vn×)(2M1+M2)+M3
W
a
b
=
f
s
f
x
b
δ
K
A
x
+
f
s
f
y
b
δ
K
A
y
+
f
s
f
z
b
δ
K
A
z
W_a^b= f_{sfx}^{b} \delta K_{Ax} + f_{sfy}^{b} \delta K_{Ay} + f_{sfz}^{b} \delta K_{Az}
Wab=fsfxbδKAx+fsfybδKAy+fsfzbδKAz
3、位置误差
δ p ˙ = M p v δ v n + M p p δ p \delta \dot p = M_{pv}\delta v^n + M_{pp}\delta p δp˙=Mpvδvn+Mppδp
= [ O 3 × 3 M p v M v p O 3 × 3 O 3 × 3 ] [ ϕ T ( δ v n ) T ( δ p n ) T ( ε b ) T ( ∇ b ) T ] = \begin{bmatrix} O_{3×3} & M_{pv} & M_{vp} & O_{3×3} & O_{3×3} \end{bmatrix} \begin{bmatrix} \phi^T \\ \\ (\delta v^n)^T \\ \\ (\delta p^n)^T \\ \\ (\varepsilon^b)^T \\ \\ (\nabla^b)^T \\ \end{bmatrix} =[O3×3MpvMvpO3×3O3×3]⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡ϕT(δvn)T(δpn)T(εb)T(∇b)T⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
其中：
M
p
v
=
[
0
1
/
R
M
h
0
s
e
c
L
/
R
N
h
0
0
0
0
1
]
,
M
p
p
=
[
0
0
−
v
N
R
M
h
2
v
E
s
e
c
L
t
a
n
L
/
R
N
h
0
−
v
E
s
e
c
L
/
R
N
h
2
0
0
0
]
M_{pv}= \begin{bmatrix} 0 & 1/R_{Mh} & 0 \\ \\ secL/R_{Nh} & 0 & 0\\ \\ 0 & 0 & 1 \\ \end{bmatrix} , \space \space M_{pp} = \begin{bmatrix} 0 & 0 & -v_N R_{Mh}^2 \\ \\ v_E secLtanL/R_{Nh} & 0 & -v_E secL/R_{Nh}^2 \\ \\ 0 & 0 & 0 \\ \end{bmatrix}
Mpv=⎣⎢⎢⎢⎢⎡0secL/RNh01/RMh00001⎦⎥⎥⎥⎥⎤, Mpp=⎣⎢⎢⎢⎢⎡0vEsecLtanL/RNh0000−vNRMh2−vEsecL/RNh20⎦⎥⎥⎥⎥⎤
综上所述
X
˙
=
F
X
+
G
W
b
=
[
M
a
a
M
a
v
M
a
p
−
C
b
n
O
3
×
3
M
v
a
M
v
v
M
v
p
O
3
×
3
C
b
n
O
3
×
3
M
p
v
M
v
p
O
3
×
3
O
3
×
3
O
3
×
3
O
3
×
3
O
3
×
3
O
3
×
3
O
3
×
3
O
3
×
3
O
3
×
3
O
3
×
3
O
3
×
3
O
3
×
3
]
[
ϕ
T
(
δ
v
n
)
T
(
δ
p
n
)
T
(
ε
b
)
T
(
∇
b
)
T
]
+
[
−
C
b
n
O
3
×
3
O
3
×
3
C
b
n
O
3
×
3
O
3
×
3
O
3
×
3
O
3
×
3
O
3
×
3
O
3
×
3
]
[
W
g
b
W
a
b
]
\dot X = FX + GW^b = \begin{bmatrix} M_{aa} & M_{av} & M_{ap} & -C_b^n & O_{3×3} \\ \\ M_{va} & M_{vv} & M_{vp} & O_{3×3} & C_b^n \\ \\ O_{3×3} & M_{pv} & M_{vp} & O_{3×3} & O_{3×3} \\ \\ O_{3×3} & O_{3×3} & O_{3×3} & O_{3×3} & O_{3×3} \\ \\ O_{3×3} & O_{3×3} & O_{3×3} & O_{3×3} & O_{3×3} \\ \end{bmatrix} \begin{bmatrix} \phi^T \\ \\ (\delta v^n)^T \\ \\ (\delta p^n)^T \\ \\ (\varepsilon^b)^T \\ \\ (\nabla^b)^T \\ \end{bmatrix} + \begin{bmatrix} -C_b^n & O_{3×3}\\ \\ O_{3×3} & C_b^n \\ \\ O_{3×3} & O_{3×3} \\ \\ O_{3×3} & O_{3×3} \\ \\ O_{3×3} & O_{3×3} \\ \end{bmatrix} \begin{bmatrix} W_g^b \\ \\ W_a^b \\ \end{bmatrix}
X˙=FX+GWb=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡MaaMvaO3×3O3×3O3×3MavMvvMpvO3×3O3×3MapMvpMvpO3×3O3×3−CbnO3×3O3×3O3×3O3×3O3×3CbnO3×3O3×3O3×3⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡ϕT(δvn)T(δpn)T(εb)T(∇b)T⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤+⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡−CbnO3×3O3×3O3×3O3×3O3×3CbnO3×3O3×3O3×3⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤⎣⎡WgbWab⎦⎤
此即为一种状态空间模型。
1255
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
