조선대학교 항공우주공학과항법제어 및 응용연구실20092984김종명

I. Introduction

II. Algorithm

III. Simulation & Result

IV. Future Plan

Research purpose

3

3

2

1

0.1

1

0.02

0.01

x

y

x

y

x

y

xp

y

vv

v

aa

a

Ideal Value

3.1

2.9

1.5

1.2

0.05

1.3

0.011

0.021

x

y

x

y

x

y

xp

y

vv

v

aa

a

Sensor output

Research purpose

Output Ideal

: Bias, Noise,…

Output Optimal Output

Research purpose

MagnetometerAccelerometer

Gyro

ExtendedKalman Filter

INS(관성항법시스템) 알고리즘 구성도

𝜔𝑏

Measurement

𝑓𝑏 𝑚𝑏

State Update

EstimationAttitude

Norm AnalysisR Update

1. System Model

1( )

2q q

0

0[ ]( )

00

0

z y x

z x y

Ty x z

x y z

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

x F t x G t w t

t IF t

IG t

I

IQ t

I

2. Extended Kalman Filter

1( )

2q q

2. Extended Kalman Filter

Propagation

1ˆˆ ˆ( )k k kq q

3 3

1ˆ ˆ ˆcos [ ]

2ˆ( )

1ˆ ˆcos

2

k k k

k

T

k k

t I

t

1ˆ ˆsin

2ˆ

ˆ

k k

k

k

t

(Transform matrix),k k

1

T T

k k k k k k kP P Q

2. Extended Kalman Filter

1

ˆ ˆ ˆ( ) ( ) ( )T T

K k K k k k k k kK P H x H x P H x R

1 3 3

2 3 3

ˆ( ) 0ˆ( )

ˆ( ) 0k

K k

t

A q rH x

A q r

Gain

1

2

n

n

r m

r a

2. Extended Kalman Filter

Update

1 1ˆ( )T

k k K k kP I K H x P

ˆ ˆ( )k k k k kx K y h x

ˆT

T T

k k kx

1

2

ˆ( )ˆ( )

ˆ( )k

k k

t

A q rh x

A q r

2. Extended Kalman Filter

Update

1ˆ ˆ ˆ( )

2k k k kq q q

4 3 3 1 3

1 3

[ ]ˆ( )k T

q I qq

q

ˆ ˆ ˆk k k

re-nomarlize quaternion

3. Norm Analysis

2 2 2

ˆ ˆ

ˆ ˆ

T

x y z x y z x y z

T

k k k k

T

k k k k

T

f f f f f f f f f

H x v H x v

trace H x v H x v

trace HPH R

2 2 2

( ) ,

( ) x y z

if abs g norm f thr then Measurement update

norm f f f f

3. Norm Analysis

1 2 3 3ˆ( ) 0H A q r

2 2 2

1 1

T

x y z af f f trace H PH R

2 2 2

1 1 1T

x y z af f f trace H PH R

Simulation A

State : QuaternionState Update : GyroMeasurement : Accelerometer & MagnetometerFilter : EKF

Simulation B

State : QuaternionState Update : GyroMeasurement : Accelerometer & MagnetometerFilter : EKF + Norm Analysis

Simulation C

State : QuaternionState Update : GyroMeasurement : Accelerometer & MagnetometerFilter : EKF + Norm Analysis + R Update

Simulation A

Simulation B

Estimate & trueAttitude

Simulation C

Estimate & trueAttitude

Simulation A

Simulation B

Angular rate

Simulation C

Angular rate

Simulation A

Simulation B

Attitude Error

Simulation C

Attitude Error

Simulation A

Simulation B

Bias Error

Simulation C

Bias Error

Result

1. 3가지 방법에 대한 시뮬레이션과 그에 따른 결과 도출을 성공함.

2. 기존의 방법보다 오차가 작음을 확인함

1. Unscented Kalman Filter

2. Particle Kalman Filter

3. Estimate Attitude

Q & A