[칼만필터] Chap 9. 위치로 속도 추정하기

 Chap 9. 위치로 속도 추정하기

https://github.com/tbmoon/kalman_filter/tree/master/Ch09.Pos2VelKF

참고하였습니다!

 

 

열차문제

직선 선로에서 80m/s 속도 유지. 위치, 속도 정보는 0.1s 간격으로 저장

속도 데이터를 저장하지 못함. 위치 정보로 속도정보를 얻어내기 

 

9.1 시스템 모델

속도는 이동거리를 시간으로 나눈 값.

 

원하는 상태변수

\(x = \begin{bmatrix}위치 \\ 속도 \end{bmatrix} \)

 

관계식부터 정의해보면

\( 위치_{k+1} = 위치_{k} + 속도_{k} * \Delta t\)

\( 속도_{k+1} = 속도_{k} + w_{k} \)

\(w_k\)는 시스템 잡음.

 

관계식만 생각해보면 당연하다. 속도에는 잡음이 낄 수 밖에 없고.. 잡음을 적절하게 적용하는 위치도 고려할 수 있는 능력이 필요할 것 같다. 

 

시스템 모델식에 위의 관계식을 넣어보자

\(x_{k+1} = A x_{k} + w_{k} \)

 

상태변수 x는 이미 위치, 속도로 사용할 것으로 결정했고, 뽑아내어야할 값도 상태변수 x이다.

 

\( \begin{bmatrix}위치 \\ 속도 \end{bmatrix}_{k+1}= \begin{bmatrix}1 & \Delta t \\ 0 & 1 \end{bmatrix} \begin{bmatrix}위치 \\ 속도 \end{bmatrix}+ \begin{bmatrix}0 \\ w_{k} \end{bmatrix}  \)

 

딱 위에서 정의해논 관계식이 고대로 A 에 들어갔다. 

 

 

\(z_{k} = H x_{k} + v_{k}\)

문제에서 측정된 값은 위치만이라고 하였다. 

 

\( z_{k} = \begin{bmatrix}1 & 0 \end{bmatrix} \begin{bmatrix}위치 \\ 속도 \end{bmatrix}_{k} + v_{k} \)

\(v_{k}\)는 측정잡음

\(H = \begin{bmatrix}1 & 0 \end{bmatrix} \)

 

일단 설정한 문제와 동일하게 세팅된 거 같다. 

 

마지막으로 잡음의 공분산 행렬을 결정해주면 된다. 

측정잡음 \(v_{k}\)는 센서 제작사에서 제공해주지만 보통은 실험을 통해서 결정해야한다. 

또한, 시스템 잡음 \(w_k\)는 경험에 의존해야한다. 

(위 2개의 잡음설정은 어떻게 하는지는 나중에 알아보자..)

 

\(Q = \begin{bmatrix}1 & 0 \\ 0 & 3 \end{bmatrix} \)
\(R = 10\)

 

9.2 이후 코드결과

 

실제 측정과 예측..비교. 거의 들어맞는다.

 

 

초기 속도값을 20m/s로 시작하니 약 2초 정도 지나서야 따라붙는다.

 

 

9.4 속도로 위치 추정하기 

시스템 모델을 좀 다르게 수정하면 속도로 위치를 추정하는 것도 가능하다. 

\( z_{k} = \begin{bmatrix}0 & 1 \end{bmatrix} \begin{bmatrix}위치 \\ 속도 \end{bmatrix}_{k} + v_{k} \)

\(H = \begin{bmatrix} 0 & 1 \end{bmatrix} \)

 

 

행렬 하나 바꿔서 얻는 값을 바꿀 수도 있다.. 참 편리하네. 

내가 원하는 상태변수들, 사용해야하는 변수들을 잘 선정하는 것이 중요하다. 

속도값에서 추정값은 0에서부터주욱 올라가기 시작하는데 이부분도 염두해두는게 좋겠다. 

실제 사용에서는 약간의 안정화시간이 필요할지도 모르겠다. 

 

9.5 초음파 위치로 속도 측정하기 

 속도가 일정하지 않은 경우에서 테스트

시스템의 기동은 다르지만, 앞의 예제와 같은 시스템 모델임.(=물리적인 관계는 안변함)

 

 

9.6 효율적인 칼만필터함수

 \(K_k = P_{k}^{-} H^{T}(H P_{k}^{-}H^{T}+ R)^{-1}\)

 

실제 시스템 모델 H를 대입해서 칼만게인을 계산하는 공식부분을 단순화하는 것이 가능하다. 

사용되는 모델에 따라서 달라지겠지만, 실제 적용시에는 이런식으로 식 최적화가 있으면 좋긴하겠다.

 

 

 

9.7 시스템 모델의 힘

 

칼만필터는 시스템 모델을 알고있기 때문에 입력값이 있다면, 시스템이 정확히 어떤 값을 출력하는지 알고있다는 전제가 있다. 상태변수 사이의 연관 관계를 나타내는 시스템 모델을 통해서 측정하지 않은 상태변수를 추정해내는 것이 가능하다. 

 

측정위치에는 잡음이 있고, 오차가 있기 마련이지만, 그 값이 참값 근처에 머물기 마련이다. 따라서 추정된 결과도 시스템의 물리법칙을 따르기 때문에 칼만필터로 이를 표현할 수 있는것이다. 

 

시스템 모델이 잘못되면 추정결과가 엉망이된다. 

 

=> 아주 맞는 말이다. 모델이 결정되면 이를 바탕으로 추정하는 필터이니까. 

결국 모델링을 잘하는 것이 중요한데, 웬만한 사용처에 대해서는 논문들에 모델링이 있을 것이고, 

새로 구축해야하는 모델에 대해서는 물리적인 이해를 높힌다음에 접근하는것이 좋겠다.