Korkens styrkort för multirotors (KFly) - Nya bilder s. 27!

[Korken]

Godagens!

Har tyvärr haft mycket att göra föra att förbereda inför mitt exjobb så det här har tyvärr stått stilla.
Men! Nu så kommer jag med lite info samt min Quaternion AHRS Estimerare medans jag är ledig över jul.

Först svarar jag på lite frågor...

Andax: Jag har deisgnat ett system som automatiskt kalibrerar min accellerometer. Nu mäter den inom (vad jag har kunnat testa till) +/- 0.1% av en g.
Jag skapade en Gauss-Newton optimeringsmodell för sensorn enligt denna formel:
ax, ay, az är den uppmätta accelerationen.
mx, my, mz är offset av alla tre axlar.
gx, gy, gz är varje axels gain.

Ligger sensorn stilla får man då att summan av kvadraten av alla axlar ska vara lika med 1g:

Kod: Markera allt

           2            2            2 
  (ax - mx)    (ay - my)    (az - mz) 
  ---------- + ---------- + ----------  =  1
        2            2            2 
      gx           gy           gz

Kör man sedan med Gauss-Newtons itterativa lösningmetod på detta så får man ut extremt bra värden av offset och gain.
Samt man behöver typ bara 50-100 mätpunkter av varje sida på sensorn. Så man behöver ta in data på 6 olika lägen för att extremt bra kunna estimera parametrarna!
Jag till denna kalibrering i Config-programmet för att man enkelt ska kunna kalibrera sensorn.

Sedan va det de om min Quaternion estimator.
Jag läste igenom tråden och insåg att jag glömt lägga upp dokumentet! Fy på mig.
Så här kommer det! Läs igenom och kom gärna med förbättringar.
Jag funderar lite om kvällarna att dumpa EKF (Extended Kalman Filter) tänket och gå över till UKF (Uncented Kalman Filter).
Då min största olinjaritet är state*state så skulle ett UKF perfekt estimera allt, medans ett EKF bara ger en första ordningens approximation.

kalman-quaternion-AHRS.pdf

[Andax]

Tack för uppdateringen. Läste om UKF för många år sedan. Mycket intressant. Ser fram emot vad du kommer fram till.

[Andax]

En annan fråga: finns det någon vettig metod att kalibrera gyrona också?

[Agwan]

Kalibrerar inte gyrona sig själva genom kalmanfiltret? Det är vad jag gör iaf. Fast när jag skummade igenom Korkens rapport så verkar han och jag ha gjort lite olika. Men jag kan ha skummat lite för fort.

Jag låter gyrona driva systemet och sedan kör jag mätningar med accelerometern. Mätningarna från accelerometern korrigerar då i mitt fall för biasen hos gyrona. Jag kör på en 8-bitars AVR, så jag har inte kraft att ha för stor systemmatris. Men gyro-gainet skall kunna korrigeras så också. Med en magnetometer så blir det ännu finare eftersom accelerometrarna inte mäter mot Z-gyrot speciellt mycket i många applikationer och då har Z-gyrot en tendens att få fel gyro/gain. Speciellt om man estimerar båda eftersom gyrogain = 0 gör att biasen kan bli vad som helst.

Vill man kalibrera in gyrona så behöver man något som roterar med en konstant men ställbar hastighet. Det är lite jobbigt, man behöver batterier och kablar. Men jag tror jag beskrev tidigare i den här tråden hur jag gjorde det med en kub istället.

[malx]

För att inte glömma temperaturkammare eftersom gyrobias ändras med såväl rotationshastighet och temperatur. Ett ganska omfattande jobb, det är en av orsakerna de bättre IMU:erna kostar så mycket.

Korkens kalmanfilter uppskattar precis som du säger gyrobiaset. En bra kompromiss i sammanhanget även om bra utrustning och noggrann manuell kalibrering kan ge bättre resultat.

[Korken]

Som ni säger så låter jag filtret kalibrera bort Gyro-bias.
Man kan göra som malx säger och mäta upp bias och använda detta som startposition för filtret, det gör att filtret konvergerar mycket snabbare.

När det kommer till gain i Gyrona så ska de vara kalibrerade till inom +/- 0.5%, om jag inte missminner mig, för hela temperaturområdet
Detta påverkar nästan inget då dessa fel ackumulerar väldigt sakta så felet kan motverkas med gyrobias.
Jag testade mina gyron genom att rotera en kub 90 grader och då fick jag 16.36 i känslighet när det va 16.4 enligt datablad genom att integrera, dock inte med en trapetsiodal integration så det hade gjort testet ännu bättre (körde med x(k+1) = x(k) + z(k)*dt).
Så gyrona är mycket bättre kalibrerade än accelerometrar.

[BBQ]

Impad över din reglerteknik-kunskaper!

Så här kan skalfaktorvariationen se ut för ett CRM100 gyro över temperatur.

Fel(%) = (1-(ideal sf/measured sf))*100

[Korken]

>> Impad över din reglerteknik-kunskaper!
Tackar! Jag hoppas bara att det räcker för att impa på mina professorer så att jag får doktorera inom reglerteknik också.

>> Så här kan skalfaktorvariationen se ut för ett CRM100 gyro över temperatur.
Mycket intressant! Jag har aldrig sätt verkliga mätningar på detta.
Bra att du la upp det också! Jag ser nu att jag har kollat på fel värden i databladet.
MPU-6050 ligger på +/- 3% i SF. Men men, alla tester jag har gjort så hinner gyro bias kompensera för felet som gainet ger upphov till.

[Andax]

Ja det var lite min frågeställning. Bias kommer in linjärt och funkar fint att skatta via Kalman-filtret. Skalfaktorn kommer in olinjärt och skattas inte lika enkelt. Dock skrev du att du kört med EKF och framöver UKF. Då kanske det ter sig enklare att skatta korrekt bias OCH skalfaktor för gyrot.

[Andax]

Ja det var lite det jag menade. Bias kommer in linjärt och funkar fint att skatta via Kalman-filtret. Skalfaktorn kommer in olinjärt och skattas inte lika enkelt. Dock skrev du att du kört med EKF och framöver UKF. Då kanske det ter sig enklare att skatta korrekt bias OCH skalfaktor för gyrot.

[Korken]

Hmm, jag vet inte riktigt hur man skulle göra för att uppskatta skalfaktorn.
Men ja tror säkerligen att det går, det enda är att den kommer nog vara ganska instabil så man tror mest på gyrot och sedan vill man kalibrera gyrot via acc/mag som man tror mindre på.
Så man får nog fundera på hur man ska modellera skalfaktorn i resten av modellen.

Det fina med att uppskatta bias/skalfaktorn är att komplexiteten av filtret stiger inte, så med väldigt lite mer beräkningar kan man nog få med det.
(Detta är pga att inversen styrs av antalet mätningar man har.)

Men, har du någon bra modell på hur man ska uppskatta skalfaktorn?
Jag kan på rak arm inte komma på något, men jag tycker att det borde gå.

EDIT:
Det som slog mig nu, kollar man i mitt dokument i h(x) borde det inte räcka med att utöka rad 3-6 i vektorn såhär?
(w + w_b) till (w + w_b)*w_g där w_g är gainet.
Då borde det inkluderas i resten av modellen, samt sedan utöka state-vektorn med w_gx, w_gy och w_gz.

EDIT2:
Lägger man till det jag skrev innan så blir EKF formuleringen såhär, ser ni något fel med matriserna sådär på rak arm?

Kod: Markera allt

x =
 
  q0
  q1
  q2
  q3
  wx
  wy
  wz
 wxb
 wyb
 wzb
 wxg
 wyg
 wzg
 
 
f =
 
 q0 + (dt*q1*wx)/2 + (dt*q2*wy)/2 + (dt*q3*wz)/2
 q1 - (dt*q0*wx)/2 + (dt*q2*wz)/2 - (dt*q3*wy)/2
 q2 - (dt*q0*wy)/2 - (dt*q1*wz)/2 + (dt*q3*wx)/2
 q3 - (dt*q0*wz)/2 + (dt*q1*wy)/2 - (dt*q2*wx)/2
                                             wx
                                             wy
                                             wz
                                            wxb
                                            wyb
                                            wzb
                                            wxg
                                            wyg
                                            wzg
 
 
h =
 
                                       2*q0*q2 - 2*q1*q3
                                     - 2*q0*q1 - 2*q2*q3
                             - q0^2 + q1^2 + q2^2 - q3^2
                                          wxg*(wx + wxb)
                                          wyg*(wy + wyb)
                                          wzg*(wz + wzb)
 bx*(q0^2 + q1^2 - q2^2 - q3^2) - bz*(2*q0*q2 - 2*q1*q3)
         bz*(2*q0*q1 + 2*q2*q3) - bx*(2*q0*q3 - 2*q1*q2)
 bz*(q0^2 - q1^2 - q2^2 + q3^2) + bx*(2*q0*q2 + 2*q1*q3)
 
 
Fk =
 
[          1,  (dt*wx)/2,  (dt*wy)/2,  (dt*wz)/2,  (dt*q1)/2,  (dt*q2)/2,  (dt*q3)/2, 0, 0, 0, 0, 0, 0]
[ -(dt*wx)/2,          1,  (dt*wz)/2, -(dt*wy)/2, -(dt*q0)/2, -(dt*q3)/2,  (dt*q2)/2, 0, 0, 0, 0, 0, 0]
[ -(dt*wy)/2, -(dt*wz)/2,          1,  (dt*wx)/2,  (dt*q3)/2, -(dt*q0)/2, -(dt*q1)/2, 0, 0, 0, 0, 0, 0]
[ -(dt*wz)/2,  (dt*wy)/2, -(dt*wx)/2,          1, -(dt*q2)/2,  (dt*q1)/2, -(dt*q0)/2, 0, 0, 0, 0, 0, 0]
[          0,          0,          0,          0,          1,          0,          0, 0, 0, 0, 0, 0, 0]
[          0,          0,          0,          0,          0,          1,          0, 0, 0, 0, 0, 0, 0]
[          0,          0,          0,          0,          0,          0,          1, 0, 0, 0, 0, 0, 0]
[          0,          0,          0,          0,          0,          0,          0, 1, 0, 0, 0, 0, 0]
[          0,          0,          0,          0,          0,          0,          0, 0, 1, 0, 0, 0, 0]
[          0,          0,          0,          0,          0,          0,          0, 0, 0, 1, 0, 0, 0]
[          0,          0,          0,          0,          0,          0,          0, 0, 0, 0, 1, 0, 0]
[          0,          0,          0,          0,          0,          0,          0, 0, 0, 0, 0, 1, 0]
[          0,          0,          0,          0,          0,          0,          0, 0, 0, 0, 0, 0, 1]
 
 
Hk =
 
[              2*q2,             -2*q3,                2*q0,             -2*q1,   0,   0,   0,   0,   0,   0,        0,        0,        0]
[             -2*q1,             -2*q0,               -2*q3,             -2*q2,   0,   0,   0,   0,   0,   0,        0,        0,        0]
[             -2*q0,              2*q1,                2*q2,             -2*q3,   0,   0,   0,   0,   0,   0,        0,        0,        0]
[                 0,                 0,                   0,                 0, wxg,   0,   0, wxg,   0,   0, wx + wxb,        0,        0]
[                 0,                 0,                   0,                 0,   0, wyg,   0,   0, wyg,   0,        0, wy + wyb,        0]
[                 0,                 0,                   0,                 0,   0,   0, wzg,   0,   0, wzg,        0,        0, wz + wzb]
[ 2*bx*q0 - 2*bz*q2, 2*bx*q1 + 2*bz*q3, - 2*bx*q2 - 2*bz*q0, 2*bz*q1 - 2*bx*q3,   0,   0,   0,   0,   0,   0,        0,        0,        0]
[ 2*bz*q1 - 2*bx*q3, 2*bx*q2 + 2*bz*q0,   2*bx*q1 + 2*bz*q3, 2*bz*q2 - 2*bx*q0,   0,   0,   0,   0,   0,   0,        0,        0,        0]
[ 2*bx*q2 + 2*bz*q0, 2*bx*q3 - 2*bz*q1,   2*bx*q0 - 2*bz*q2, 2*bx*q1 + 2*bz*q3,   0,   0,   0,   0,   0,   0,        0,        0,        0]

[Agwan]

När jag gjorde mitt exjobb så fick jag lära mig att det var mycket bättre att skippa själva vinklarna och positionerna i statevektorn och bara estimera felparametrarna. Sedan köra ett separat system på sidan med bara gyron i, och sno felparametrarna från kalmanfiltret. Det är mycket lättare för filtret att estimera fel än en parameter med fel på, när parametern i sig också ändras.

Killarna på autonoma system på FOI höll på och differentierade systemekvationerna och ut ramlade felparametrar åt höger och vänster som då skulle vara bias och gain. Jag förstod aldrig riktigt det där då, och har inte satt mig med det mer sedan dess heller. Det kanske inte är så svårt bara man tittar på det...

De snackade vagt om UKF också, men det tittade jag aldrig på så jag vet inte vad skillnaden är riktigt.

Olinjäriteter i mätvektorn är inte så farligt som i statevektorn. Det är som du säger bara att lägga till gainet där som en parameter. Statevektorn blir svår eftersom man inte kan bryta ut en multiplikation där. Utan man måste linjärisera runt varje punkt va?

[Korken]

Agwan:
Ibland är det bättre att dela upp filter, dock om man måste ha den fulla olinjära modellen så kan man inte göra uppdelningen utan att introducera mer avrundningar och fel.

Skillnaden mellan EKF och UKF är att kovariansmatrisen i ett EKF är en 1a ordningens approximation av det riktiga fallet.
Ett UKF gör en sk. Uncented Transform på statesen viket effektivt ger en 2a ordningens approximation (enkelt förklarat och inte helt korrekt, men ger lite känsla).
Uncented Transform gör så att medelvärdet och variansen tas fram genom att låta de propagera genom olinjariteterna och på så sätt inte få en approximation på samma sätt.
Detta vart lite luddigt, men jag hoppas du förstår.



Jag testade min idé och det vart som jag trodde bias och gain kommer motvärka varandra.
Ska man också uppskatta gain så måste en annan modell än bara en mätmodell användas.
Idéer på detta?

[Agwan]

Problemet med att skatta båda är att signalerna måste röra på sig tillräckligt för att filtret skall kunna skatta parametrarna. Ett flygplan som flyger mest plant har svårt att skatta parametrarna till heading-gyrot om det inte har magnetometer. Men om det rollar och loopar mycket så är det inget problem.

[Korken]

Agwan: Problemet ligger inte bara i det. Jag testade att stå stilla då och då. Snabba rörelser, kontinuerliga rörelser och massa annat.
Men inget lyckas skatta gain och bias samtidigt. Något måste göras åt modellen.
Idéer mottages gärna.