[제어시스템공학-8] 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

 

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

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

---

이전까지는, Fix된 값에 노이즈를 주어 칼만필터로 추정, y=x값에 noise를 주어 추정해보았는데요

 

이번에는, 칼만필터 개념설명을 할때 예시로 들었던, [위치 속도] 변수로 2차원 칼만필터를 직접 구현해볼까합니다.

 

※ 과거글 내용 복사

[Ex] 등속도 운동을 하는 물체의 위치(이동거리) 찾기

x_k+1 = A*x_k + Q

z_k = H*x_k + R

 

이라는 수식이 있다고 가정해보시죠. x는 위치입니다.

 

등속도 운동을 한다는 전제가 있기에, x_k+1 = x_k 겠죠? x의 값은 언제나 동일할것입니다.

따라서 x_k=[위치;속도] 라는 2행 1열 벡터라고 했을때,

A= [1 △t;0 1]이 될것입니다.

 

왜냐구요? 한번 같이 구해보시죠

 

[현재위치;현재속도] =  [1 △t;0 1]*[과거위치;과거속도] 가 x_k+1 = A*x_k의 의미입니다.

정리하자면,

현재위치 = 과거위치 + △t*과거속도

현재속도 = 과거속도

 

가되는것이죠!

이것이 바로 "A"입니다.

---

위에서 정의한대로 현재 센싱되는 위치를 기준값으로 잡고, 현재의 속도를 추정해보겠습니다.

 

속도는 80km/h에 Noise를 주어 약간씩 변한다고 가정하였으며, 위치값을 센싱하여 이 값을 기준으로 속도값을 정확하게 추정해보는 문제입니다.

 

말이어렵네요

 

그냥, "정확한 위치"를 알고있을 때, "속도"를 추정하는 문제입니다.

 

현재 이 칼만필터는 위치값을 정확하게 알고 구현되는것이기에 실제값과 추정값이 거의 차이가 안남을 알 수 있습니다.

 

Kalman gain으로 지속적으로 Error를 보상해주기 떄문인데요,

 

따라서, 칼만필터는 센싱값으로 얻어지는 추정값 변수를 얻는것이 아니라, 센싱값 변수로 구성되어있는 다른 수식을 얻어내는데에 목적이 있습니다. (위치데이터로 수식을 짠 속도데이터를, 위치데이터만으로 알아낸다는 말)

 

그럼, 이 위치데이터로 속도를 얼마나 정확하게 추정하는지 보겠습니다.

 

 

 

붉은색 점이 찍혀있는것들이 측정된 실제속도이고

직선들이 Kalman Filter로 추정하는 속도값인데요

 

얼추 비슷하게 맞아들어가는것을 확인할 수 있습니다. (80km/h +-noise로 움직임)

초기값을 20km/h로 주어 일부로 차이를 벌려놨는데도 곧잘 따라가는모습을 확인하실 수 있습니다.

---

[코드분석-1]

 

GetPos()함수를 통해 실제 위치센싱값과, 속도 추정값을 저장하고

위치센싱값(z)를 통해 DvKalman(z)로, 칼만필터링을 하여 추정된 위치와 속도를 [pos vel]에 담는 코드입니다.

 

[코드분석-2]

 

DvKalman필터 코드입니다.

0.1초간격으로 10초간 100샘플의 데이터를 보았으며,

 

해당 코드의 Q와 R 값을 어떻게 바꿔주냐에 따라 칼만필터의 성능이 아주 달라집니다.

이하 코드들은 이전글과 동일한 칼만필터 코드입니다.

 

 

[코드분석-3]

실제 현재센싱 위치값과, 속도값을 계산하는 코드입니다.

---

이번에 구현한 예제는 간단한 state space equation을 세워서

 

"정확한 위치값을 줄때, 정확한 속도값을 추정할 수있니?"에 대답해보는 질문이였습니다.

 

 

※ 참고

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