출처 : http://blog.naver.com/chogaehwan/220771221809
확장칼만필터로 레이다추적
측정값이 바로 물체까지의 거리이다.
이것을 갖고 고도와 직선방향거리, 속도를 추정해보는 것을 해보자
물체까지의 거리가 비선형인 케이스
거리는 이 코드의 매트랩function을 통해 받는다.
이렇게 모델링
생각보다 간단하게 됐네…
결과는 좋다..
주요한건 역시 H를 저런 식으로 구해야 한다는 것이다. 예측값에서의 선형화를 통해 저렇게 구한다..
선형시스템에서는 H가 측정값과 state와의 관계를 나타내는 값이었는데…저런식으로 된다…h(x)를 편미분해서 예측값을 넣은거지. 왜 직전 추정값을 넣지 않고 예측값을 넣었을까?
직전 추정값보다는 현재 예측값이 더 현재값에 근접하다고 판단할 수 있기 때문
선형시스템에서 z=H*xp+v는 xp를 통해 z를 계산한 것이다. h도 마찬가지다. 함수이고. H는 그 함수에 각각 성분으로 편미분하고 대입하여 구한 값이다. 여기서 H가 바로 자코비안 행렬이라는 것이다.
http://darkpgmr.tistory.com/132 이걸 참고해도 좋다..
결과는
x_trk
h_alt 완전히 1000m까지는 안 가네?
velo 왔다갔다하는군…참값은 100
거리를 측정값과 추정값 비교한 것…잘 나온다 와우