ivme gyro sensoruyle açı hesaplama

[koray692]

merhaba arkadaşlar ivme sensoruyle ve ayırca ivme ve gyro sensörünü tamamlayıcı filitreyle hesaplatıp lcd ye açıyı yazdırıyorum ... yatay bir şekilde bordun üzerinde dururken X pozisyonundan açı değerini 90 derece okuyorum buraya kadar sıkıntı yok ancak sensorun yönünü 0 dereceye yakınsayacak şekilde oynattığımda çoğu zaman 10 dereceye yakınsıyor aynı şekilde 180 derece olacak şekilde döndürdüğüm zamanda çoğu zaman 175 e yakınsıyor..Çoğu zaman diyorum çünkü çok ilginç birşekilde hiçbirşeyde değişiklik yapmadığım halde yakınsama değerleri değişebiliyor 1 derece ve 179 dereceyi görebiliyorum...tamamlayıcı filitre de bunu kullandım aci= (a*(aci+((float)GYROx/16.0f)*dt) +(1-a)*(ACI_ACCx)) ;   dt=10 ms   a=0.5

g-zero output ların yanlış yazılmış olmasından şüpheleniyorum gerçi yazmasamda aynı sonuçlar çıkıyor hemen hemen pek etkilemiyor...

// ivmelenme(g cinsinde) hesaplama        819-xy          z-1310.72

  G_ACCx = (double)( ((float)ACCx- 819.0f )/(16384.0f)   );              //     AFS_SEL=0   16,384 LSB/g DE
  G_ACCy = (double)( ((float)ACCy- 819.0f )/(16384.0f)   );              //     AFS_SEL=0   16,384 LSB/g DE
  G_ACCz = (double)( ((float)ACCz -1310.72 )/(16384.0f)   );              //     AFS_SEL=0   16,384 LSB/g DE
  abc=(G_ACCx*G_ACCx) + (G_ACCy*G_ACCy) + (G_ACCz*G_ACCz);

  G_ACC=(double) sqrt(abc);                                                // R^2 = Rx^2 + Ry^2 + Rz^2
  ACI_ACCx=  57.2958*acos(G_ACCx / G_ACC);                                 // Axr = arccos(Rx/R)
  ACI_ACCy= 57.2958* acos(G_ACCy / G_ACC);                                 // Ayr = arccos(Ry/R)
  ACI_ACCz=  57.2958*acos(G_ACCz / G_ACC);   

[berat23]

hep aynı soruyu soruyorsunuz, ben bir kez cevap yazmıştm, orda yeterli bilgi var aslında.

şimdi yanlışlardan başlayaım;
1. sadece atan için ivmelerin birmlerini ms2 yapmak zorunda değilsiniz.(bu hata değil, gereksiz işlem)
2. 90 zaten euler için singularity(bkz:tanjant) ayrıca gyro lock olur sonrasında.
3. gyrodan okuduğun değerler body axis, onları yere çevirmen lazım(bkz: dcm)
4. eksen olarak ne aldın bilmiyorum ama z açısı dediğin azimuth(yaw) ise onu ivme ile hesaplayamazsın. eksenlerini yere göre ve açılarını çizer misin?
5. x ve y eksenlerinde tanımladığın ivme açısı hatalı. x ekseni etrafındaki dönüşün yarattığı açı = atan2(accy, accx + accz) y eskenindeki dönüş = atan2(accy, toplam  ivme) olacak.
6. complementary filter yaptığın gibi biirşey değil. sen doğrudan ortalama almışsın.aslında yapman gereken farkı bir katsayı ile çarpıp toplaman.
7. 0.5 katsayısı bence çok büyük.(filtreyi doğru kullansan dahi)

anlattıklarımın en sade ve her bilginin olduğu hali şurda;
http://www.mdpi.com/1424-8220/11/4/3816/htm

burda senin kısımların denklem 1,3 ve filtre
bu abinin tamamlayıcı filtresi şu;


burda error 1/(s+1) gibi birşeyle çarpıyor, sen bunu sabit katsayı yapacaksın sadece. açı outputun da phiM olacak.

[koray692]

merhabalar teşekkür ederim ikinci kez cevap yazdığınız için ;

Atan2 fonksiyonuyla açıları buluyorum ancak mesela x ekseni etrafında döndürürken(y sabit) sıkıntı olmuyor ancak aynı anda y etrafında da çevirmeye başlayınca x in değeleri  farklı değerler alıyor y nin bazı açı değerlerinde nedenini tam olarak anlayamadım...

birde " gyrodan okuduğun değerler body axis, onları yere çevirmen lazım" bu cümlede ne demek istediğinizi tam anlayamadım

[berat23]

aslında sorduğun cümle diğer sorunun cevabı. gövdeye bağlı gyro, gövdeye göre ölçüm yapar.bizim istediğimiz açılarda aslında yere göre. x ekseninde dönmüşken y de yaptığın hareket, düzken yaptığın gibi ölçülmez, onu yere döndürmen lazım. ham sensör ölümlerini dcm matrisi ile çarparsan gövdeye göre açısal hızları yere yani istediğimiz açının düzlemine döndürmüş olursun. kabaca bu.

[koray692]

http://www.psithetaphi.com

burda dcm matrisi ve nasıl euler e geçileceği yazıyor tam olarak bu şekilde mi yapmam gerekiyor (ham verileri dcm matrisiyle çarpın demiştiniz dcm matrisinin içinde cos(teta) sin(teta ) gibi değerler var ) nasıl olacak tam anlayamadım açıkcası...matrise göre;  ham değerleri cos(teta ) gibi değerlerle çarpıp sonra tekrar açı hesaplatıyoruz tam olarak böylemi oluyor ?

[berat23]

orda sadece dcm anlatılmış, doğru. gyrodan euler e geçiş yok. tüm olay için;
http://www.mdpi.com/1424-8220/11/4/3816/htm

gyro aslında eksenlere göre dönü ölçer fakat kendi eksenine göre. yani gyrodan aldığın q nun integrali aslında pitch olur fakat roll sıfırsa. gyrodan aldığın q p r yi dcm ile çarparsan onu yere göre döndürmüş olursun ve bulduğun değerler o açıların türevidir. hep aynı şeyi yazıyorum ama özeti bu. mesela verdiğin linkte the euler angles kısmında her bir eksen için rotasyon matrisini yazmış.