Upload
others
View
3
Download
0
Embed Size (px)
Citation preview
Dept. of Aerospace Engineering
Lab. for Navigation, Control and Applications
Park, Seong-Yong
Date: 2015. 04. 04. SAT.
1. Intro.
2. Attitude Kinematics
3. Extended Kalman Filter
4. Conclusion
5. Next research issue
- 항공기, 인공위성 등 임무를 위해 하늘에 떠있는 물체들은
정확한 임무를 수행하기 위해 자세제어가 필요함.
- 우주공간에 있어 우리 눈에 보이지 않는 인공위성의 경우
자세제어를 위해서 정확한 자세를 아는 것이 중요함.
- 기존에 많이, 널리 연구된 인공위성의 자세 추정 기법을 연구하고
시뮬레이션 해보는 것을 목표로 하였다.
T
x y zr r r r
T
x y zb b b b
b Ar
1 0 0
0 cos sin
0 sin cos
r r
cos 0 sin
0 1 0
sin 0 cos
r r
cos sin 0
sin cos 0
0 0 1
b r
c c s c c s s s s c s c
s c c c s s s c s s s c
s c s c c
A
b Ar
만약 Euler angle이 충분히 작다면, 𝑐𝑜𝑠𝜃 = 1, 𝑠𝑖𝑛𝜃 = 𝜃 으로 근사 가능.
3 3
1
1 ,
1
T
A I α α
3 2
3 1
2 1
0
0
0
α
α β α β
는 cross-product matrix 또는 skew-symmetric matrix 라고 한다. α
quaternion definition
4
TT q q p
- quaternion은 유용한 attitude parameterization 중의 하나이다.
- attitude matrix가 삼각함수로 이루어져 있기 때문에
특정 각도 (ex 90°, 270°) 에서 singularity 발생.
- quaternion의 자세 계산은 간단한 matrix연산으로 이루어지므로
euler 보다 계산량을 줄일 수 있음.
1 2 3ˆsin( / 2)
Tq q q p e 4 sin( / 2)q
: 1Tsingle constraint q q
1
4q
pq
4, / 2 1if small angles and q p αEular axis and angle
4 3 3
4
[ ]T
q
q
I pq q q q
p
quaternion kinematics model
1 1( ) ( )
2 2 q q ω ω q
consider true quaternion and estimated quaternion
0 0 0.001 0.999T
q ˆ 0001T
q
1ˆ δq q q
error quaternion
1 1ˆ ˆ δq q q q q
1 1ˆ ˆ ˆ ˆ ˆ( ) ( )
2 2 q q ω ω q
ˆ( )
0T
ω ωω
ω
estimated quaternion kinematics model
time derivative gives
1
1 0 0 0 0 0
0 1 0 0 0 0ˆ ˆ
0 0 1 0 0 0
0 0 0 1 1 1
q q
1 1ˆ ˆ ˆ ˆ 0 q q q q
1 11ˆ ˆ ˆ ˆ( ) 0
2
ω q q q q
1
ˆ ˆ ˆ0 ˆ0
ˆ ˆ ˆ0 ˆ ˆ01 1ˆ ˆ( )
ˆ ˆ ˆ0 ˆ0 02 2
ˆ ˆ ˆ 0 1 0
z y x x
z x y y
y x z z
x y z
ω q q
1ˆ1
ˆ ˆ 002
ωq q
1 1ˆ1
ˆ ˆ02
ωq q
define error angular velocity
1 1ˆ ˆ δq q q q q
1 1ˆ ˆ1 1 1
ˆ ˆ0 0 0 02 2 2
ω ω ω ωδq q q q q δq δq
ˆ δω ω ω
ˆ ω ω δω
ˆ ˆ ˆ ˆ1 1 1
0 0 0 0 02 2 2
ω δω ω ω ω δωδq δq δq δq δq δq
consider helpful identities
ˆˆ( )
0
ωω δq δq
ˆˆ( )
0
ωω δq δq
some algebraic manipulations
ˆ 1
020
δωω δpδq δq
linearized model
first-order approximation given
1 1
0 02 2
δω δωδq
assume that the true quaternion is “close” to the estimated quaternion
1
ˆ2
δp ω δp δω
4 0q δ
4 1q δ
estimated angular velocity
assume zero-mean Gaussian white-noise process
widely used rate-integrating gyro model is first order Markov process
v ω ω β η
uβ η
2
3 3v v η σ I
2
3 3u u η σ I
ˆˆ ω ω β
ˆ 0βˆˆ ( ) ( ) ( )v v δω ω ω ω β η ω β β η
1 1
ˆ ˆ ( )2 2
v δp ω δp δω ω δp β η
/ 2, / 2 p α δp δα
small angle approximation
ˆ ( )v δα ω δα β η
the EKF error model
( ) ( ) ( ) ( ) ( )t t t t t x F x G w
( ) ( ) ( ) , ( ) ( )T T
T T T T
v ut t t t t x δα β w η η
3 3
3 3 3 3
3 3 3 3
3 3 3 3
2
3 3 3 3
2
3 3 3 3
ˆ ( )( )
0 0
0( )
0
0( )
0
v
u
tt
t
t
ω IF
IG
I
σ IQ
σ I
1 1
2 2
( )
( )( )
( )kk
k k k k
n n tt
A q r ν
A q r νy h x v
A q r ν
2 2 2
1 3 3 2 3 3 3 3ndiag R σ I σ I σ I
actual attitude matrix is related to the propagated attitude1ˆ( ) ( ) ( )A q A δq A q
first-order approximation given
3 3 3 3
1
1 , ( )
1
A I α A δq I δα
a single sensor the true and estimated body vectorsˆ ˆ( ) , ( ) b A q r b A q r
3 3ˆ ˆ ˆ ˆ( ) ( ) ( ) ( )
b b b I δα A q r A q r A q r δα
sensitivity matrix
13 3
2 3 3
3 3
ˆ( ) 0
ˆ( ) 0ˆ( )
0ˆ( )k
k k
nt
A q r
A q rH x
A q r
final part is quaternion and bias update
is measurement output
ˆ ˆ( )k k k k k
x K y h x
ˆ ˆˆT
T T
k k k
x δα δβ
ky
estimate output
1
2
ˆ( )
ˆ( )ˆ( )
ˆ( )k
k k
nt
A q r
A q rh x
A q r
gyro bias update
ˆ ˆ ˆk k k
β β β
first-order quaternion update
using the quaternion multiplication rule
1ˆ
ˆ ˆ2
1
k
k k
δαq q
1ˆ 1
ˆ ˆ ˆ ˆ ˆ( )22
1
k
k k k k k
δαq q q q δα
( ) ( ) q q q q q q q q
4 3 3 4 3 3( ) , ( )T T
q q
I p I pq q
p p
if the departure from unit norm is greater then a small fraction of the
noise level (say 10−7), then should be reinitializedˆq
ˆˆ
ˆ
q
Propagation Update
a priori a posteriori
- Spacecraft Attitude Estimation을 위한 기본 개념들을 익힘.
(attitude marix, quaternion, extended kalman filter)
- EKF 구조를 만들고 있는 수식들을 직접 풀어서 증명해봄으로써
각 variable이 나타내는 의미를 정확히 파악할 수 있었다.
- attitude estimation simulation을 위한 기본 개념, 수식과 의미설명
그리고 estimation flow를 제시함으로써, 앞으로 simulation을 수행
하고자 하는 사람에게 조금이나마 도움이 될 것으로 판단함.
- Spacecraft Attitude Estimation Simulation result with EKF
(논문. Spacecrft Attitude Estimation with Low-Cost Sensors)
- Reaction wheel dynamics를 추가한 Simulation 을 수행.
- Deep Space Navigation using Pulsar survey and study.