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

we are here!

- 항공기, 인공위성 등 임무를 위해 하늘에 떠있는 물체들은

정확한 임무를 수행하기 위해 자세제어가 필요함.

- 우주공간에 있어 우리 눈에 보이지 않는 인공위성의 경우

자세제어를 위해서 정확한 자세를 아는 것이 중요함.

- 기존에 많이, 널리 연구된 인공위성의 자세 추정 기법을 연구하고

시뮬레이션 해보는 것을 목표로 하였다.

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

ˆˆ

ˆ

qq

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.