kalman filtresi-compass modulü 0 dan 360- 360dan 0 a giderken saçmalıyor?

[zengdai]

Merhaba arkadaşlar,
Basit bir otopilot programı yapmaya çalışıyorum. Bir nokta belirleyip
compass modulünden çıkan sonuca göre dönecek miktarı belirliyor. Ama uyguladığım kalman filtresinde, gideceği noktanın değeri 360 civarında ise kalman filtresi 0dan 360 a atlamalarda şaşırıyor(360 ile 0 ın aynı şey olduğunu farkedemiyor ) ortalarda bir değer verince araç etrafında dönmeye başlıyor. Mutlaka bu problemle daha önce karşılaşan olmuştur. Kafayı yiyicem
Aracın çizdiği grafiği altta verdim yeşil gitmesi gereken açının değeri, bu değer 360 a yaklaşınca hatayı veriyor(compass taki titreşimlerden dolayı)
[IMG]http://img497.yukle.tc/thumbs/3159Adsiz.jpg[/img]

[zengdai]

Hatayı bir nebze aşabildim, bir algoritma ile sorunu aşabildim ama çokta sağlam olmadı gibi, her an bir açığı çıkabilir. Ariane 5 uzay mekiği gibi bende her an bir tane düşürebilirim yani
algoritma şöyle;

if ((pusula[eski]-pusula[yeni])>=330){
pusula[yeni]=359;
xk[eski]=359;//kalman çıktısı
}
if ((pusula[yeni]-pusula[eski])>=330){
pusula[yeni]=0;
xk[eski]=0;//kalman çıktısı
}
Selametle!

[IMG]http://img497.yukle.tc/thumbs/43ssss.jpg[/img]