횽들 보상필터 ? 상보필터를 이용해서 INS(네비게이션)이 GPS의 값을 따라 가는 걸 MATLAB 으로 구현중인데

지금 yaw 값을 테스트 하고 있는데 처음 그림처럼 INS가 대략적으로 따라가고 있어.

하지만 GPS 값이 -180도 즉 -3.14의 값이 되어 버리면 INS 값이 서서히 줄어 들다가 다시 증가된 GPS 값이랑 만나게 되.

문제점 해결 하려고 둘의 오차 값 계산 후 원에서 가까운 값으로 움직이게 만드려고 했는데

if INS - GPS > pi
        INS = INS - 2* pi;

elseif INS - GPS < -pi
        INS = INS + 2* pi;
end

이랬는데 두번째 그림처럼 yaw 값은 -pi 와 +pi 값을 넘어 가면 안되는데 이미 -pi (-3.141592) 값을 넘어 버려...

어떻게 수정해야 INS 값이 GPS 값을 따라 갈 수 있게 만들 수 있을까?


요약: INS 와 GPS 의 yaw 값이 서로 같게 어떻게 만들어 줄 수 있을까?