[제어시스템공학-11] Extended Kalman Filter(확장칼만필터) 구현

[제어시스템공학-1] Average Filter(평균필터) https://limitsinx.tistory.com/69

[제어시스템공학-2] Moving Average Filter(이동평균필터) https://limitsinx.tistory.com/70

[제어시스템공학-3] Low Pass Filter(저주파  통과필터) https://limitsinx.tistory.com/71

[제어시스템공학-4] Kalman Filter(칼만필터) 개념정리(1) https://limitsinx.tistory.com/72

[제어시스템공학-5] Kalman Filter(칼만필터) 개념정리(2) https://limitsinx.tistory.com/73

[제어시스템공학-6] Kalman Filter(칼만필터) 구현 - 쉬운예제 https://limitsinx.tistory.com/74

[제어시스템공학-7] Kalman Filter(칼만필터) 구현 - 중간예제 https://limitsinx.tistory.com/75

[제어시스템공학-8] Kalman Filter(칼만필터) 구현 - 어려운예제 https://limitsinx.tistory.com/76

[제어시스템공학-9] Kalman Filter(칼만필터) 구현 - 실전예제 https://limitsinx.tistory.com/77

[제어시스템공학-10] Extended Kalman Filter(확장칼만필터) 개념 https://limitsinx.tistory.com/78

 

※이 전글에서 정리한 코드/개념은 재설명하지 않으므로, 참고부탁드립니다

※해당 글은 PC에서 보기에 최적화 되어있습니다.

---

지난글에서는 EKF(확장칼만필터)의 개념에 대해 정리를 해보았는데요

 

이번글에서는 구현을 해보도록 하겠습니다!

 

구현할 예제는, 이제까지는 다루지 않았던 샘플인데요 꽤나 실제시스템과 비슷한 state space equation을 정의내리고 진행해보고자 합니다.

 

악필로 직접작성..

문제 정의는 이렇습니다.

 

어떤 사람이 인공위성을 향해 이동하고 있습니다.(100m/s + noise)

 

이런경우, 제가 가지고 있는 실측데이터는 오직1개 r(나와 인공위성사이의 직선거리)일때, distance, altitude, velocity의 추정값을 그려보는 문제입니다.

 

즉, 이런문제를 해결하기위해선 구해야하는 3개의 변수들과 r이 모두 각각서로 연관이 있어야겠죠?

 

일단 r = sqrt(distance^2 + altitude^2)입니다. 삼각형 대각선의 길이를 구하는 공식이죠

 

또한, altitude는 바뀌지 않는값입니다.

 

제가 어느위치에 있건 인공위성과 저와의 고도차이는 항상 유지되죠, 바뀌는것은 r값과 distance값입니다.

 

velocity는 간단하게 풀기위해 100m/s + noise로 주어보겠습니다.

 

요약하자면, 

 

 

"등속도로 움직인다는 가정하에, r값은 실시간으로 센싱이될때 distance와 altitude를 예측해보라!")

 

 

가 미션입니다.

 

state space equation

 

일단, state space equation부터 세워보겠습니다.

 

distance = 속도*dt 이죠? 이부분을 위의 수식과 같이 matrix로 작성해보았습니다.

 

속도와 고도는 이전값 그대로이기에 별다른 추가항은 없고 위에서 구해진 matrix에 최종적으로

 

[1 0 0;0 1 0;0 0 1]인 Identity matrix를 더해주면 state-space equation은 완성입니다.

 

x_k = A * x_k-1에서 EKF에서는 A가 f(x)로 바뀌지만, 해당 state space eqaution은 f(x)는 선형이기에 A를 그대로 사용합니다.

 

 

h(x) jacobian

 

따라서, h(x)는 비선형입니다.

 

r = sqrt(x1^2 + x2^2)으로, altitude와 distnace를 통해, 나와 위성사이의 직선거리를 구해주고 noise를 넣어줍니다.(센서노이즈)

 

이것을 각각의 x값에대해 편미분하여 jacobian으로 정리를 해주면, 위의 이미지처럼 최종정리가 됩니다.

 

매번 jacobian을 구해주는것이아니라, 구해진 jacobian결과값들만 넣어주면 EKF는 돌아가게 됩니다.

 

---

위에서 작성한 문제들에 대해 실제 코드로 돌려보았습니다.

 

 

현재 나의 위치는 선형에가깝게 꾸준히 바뀌고잇으며, velocity는 100m/s + noise를 주었는데 거의 근사하게 따라가는 모습을 확인할 수 있습니다.

 

고도(altitude)또한 고정된 값으로 나와야하는데 약 1000정도로 결국 최종추정을 하는 모습을 확인할 수 있었습니다.

 

센싱되는 R과 EKF로 추정한 R값을 그려서 비교해보겠습니다.

 

 

 

파란색으로 그려진것이 센서값으로 얻어진 R이고, 붉은색 부분이 EKF로 따라가며 추정하는 R값입니다.

 

매우 정확하게 잘 추정하고 있음을 확인할 수 있었습니다.

(당연합니다, 칼만게인으로 오차분만큼 끊임없이 feedback제어를 해주므로)

 

---

[코드분석-1] 

시간간격은 0.05sec단위로, 20초간의 데이터를 찍은 모습입니다.

 

[pos, vel, alt] = RadarEKF : EKF를 통해 위치,속도,고도를 추정하는 코드이며,

EKFdistance = sqrt(pos.^2 6 alt.^2)라는 함수를 통해 EKF로 얻어지는 R값을 저장합니다.

 

 

[코드분석-2] 

 

위에 문제를 정의하는 과정에서, 언급했던 모든 부분을 수식과 숫자로 그냥 적어주기만 한 모습입니다.

 

x = [위치, 속도, 고도] 값으로 초기값을 0 90 1100으로 준 코드입니다.

 

 

[코드분석-3] 

센싱한 r값을 구현한 코드입니다.

 

velocity는 100m/s + 5의 noise를 주었고

altitude는 1000 + 10의 noise를 주었습니다. 초기값이 1100임을 감안하면 약 10%의 오차를 준것입니다.

 

 

[코드분석-4] 

 

R_추정값과 , H의 jacobian을 구해주는 코드들입니다.

---

Summary

 

사람이 일정한 속도로 걸어갈때, 인공위성과의 altitude, distance(직선거리)를 구해보는 예제를 EKF를 통해 풀어보았습니다.

 

state space eqaution만 정확하게 세운다면, 어떤 시스템이든 이런식으로 EKF를 적용하여 제어를 할 수 있습니다.

 

다음글로는 비선형칼만필터의 대표적 예제중 두번째인, Unscented Kalman Filter(UKF,무향칼만필터)를 정리해보겠습니다.

 

 

※ 참고

"칼만필터는 어렵지 않아" (2019,김성필)