조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8....
Transcript of 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8....
![Page 1: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/1.jpg)
조선대학교 항공우주공학과항법제어 및 응용연구실20092984김종명
![Page 2: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/2.jpg)
I. Introduction
II. Algorithm
III. Simulation & Result
IV. Future Plan
![Page 3: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/3.jpg)
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
![Page 4: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/4.jpg)
Research purpose
Output Ideal
: Bias, Noise,…
Output Optimal Output
![Page 5: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/5.jpg)
Research purpose
MagnetometerAccelerometer
Gyro
ExtendedKalman Filter
![Page 6: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/6.jpg)
INS(관성항법시스템) 알고리즘 구성도
𝜔𝑏
Measurement
𝑓𝑏 𝑚𝑏
State Update
EstimationAttitude
Norm AnalysisR Update
![Page 7: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/7.jpg)
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
![Page 8: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/8.jpg)
2. Extended Kalman Filter
1( )
2q q
![Page 9: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/9.jpg)
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
![Page 10: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/10.jpg)
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
![Page 11: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/11.jpg)
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
![Page 12: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/12.jpg)
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
![Page 13: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/13.jpg)
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
![Page 14: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/14.jpg)
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
![Page 15: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/15.jpg)
Simulation A
State : QuaternionState Update : GyroMeasurement : Accelerometer & MagnetometerFilter : EKF
![Page 16: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/16.jpg)
Simulation B
State : QuaternionState Update : GyroMeasurement : Accelerometer & MagnetometerFilter : EKF + Norm Analysis
![Page 17: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/17.jpg)
Simulation C
State : QuaternionState Update : GyroMeasurement : Accelerometer & MagnetometerFilter : EKF + Norm Analysis + R Update
![Page 18: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/18.jpg)
Simulation A
Simulation B
Estimate & trueAttitude
![Page 19: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/19.jpg)
Simulation C
Estimate & trueAttitude
![Page 20: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/20.jpg)
Simulation A
Simulation B
Angular rate
![Page 21: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/21.jpg)
Simulation C
Angular rate
![Page 22: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/22.jpg)
Simulation A
Simulation B
Attitude Error
![Page 23: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/23.jpg)
Simulation C
Attitude Error
![Page 24: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/24.jpg)
Simulation A
Simulation B
Bias Error
![Page 25: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/25.jpg)
Simulation C
Bias Error
![Page 26: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/26.jpg)
Result
1. 3가지 방법에 대한 시뮬레이션과 그에 따른 결과 도출을 성공함.
2. 기존의 방법보다 오차가 작음을 확인함
![Page 27: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/27.jpg)
1. Unscented Kalman Filter
2. Particle Kalman Filter
3. Estimate Attitude
![Page 28: 조선대학교항공우주공학과 항법제어및응용연구실 20092984 김종명 · 2015. 8. 6. · 0.021 x y x y x y x p y v v v a a a Z Z Z ª º ª ... kk t Aq r hx Aq r](https://reader035.fdocuments.net/reader035/viewer/2022071407/60fd5f2c84c7392fc525b6e9/html5/thumbnails/28.jpg)
Q & A