Estimeringsmodell för höjd

[Korken]

Godagens!

Jag har arbetat ganska mycket på mitt nya AHRS filter för KFly, men jag har planer på att utöka den modellen med ett till Kalman-filter för att estimera höjden på ett bra sätt.
Jag tänkte skriva hur jag har tänkt göra detta så får ni komma med invändningar. Jag är inte riktigt säker på att allt detta kommer fungera som planerat, men jag hoppas att ni kanske kan komma med förbättringar.

Så här ser det ut nu.
Jag har tre sensorer som kan vägas samman för att skapa höjdmätningar:
- Sonar (fungerar inom 20cm-7m)
- Trycksensor (MS-5611)
- Accelerometer (för att få med snabba ändringar)
Alla mätningar från sonar och trycksensorn görs om till meter och accelerometerns uppmätta acceleration samt sonarns höjd har vinkelkompensering när det kommer in i filtret.

Första problemet: Acc -> Höjd (idealiserad modell)
Fixas enkelt med en dubbelintegrator med ett bias (jordacceleratioinen) som filtret får hitta.

Resten är ganska rakt fram. Jag slog ihop en PDF med alla matriser.
Dock så är jag lite fundersam på min A matris. Accelerationen är det ända som ändras fort så jag har bara en modell för det, resten får bara glida med i farten så att säga.
Skulle ni ha gjort det på något annat sätt?

Sedan funderar jag på att i tuningmatriserna "ta bort" sonarns påverkan när man är på en höjd utanför dess arbetsområde.
Vad tror ni om detta?

height_estimation.pdf

[Korken]

Jag kom på mig själv att det är ett autonomt system jag har så jag hade gjort lite fel och valt konstiga states.
Så jag gjorde om allt för att bättre spegla ett autonomt system samt så blev matriserna bättre.
Då min acceleration är en state och en mätning så borde den inte komma in via B-matrisen.

Vad tror ni om detta?
För att lägga till höjd med trycksensorn så behöver jag bara modifiera min H-matris, vilket är mycket trevligt.

ny.png

[Korken]

Ojoj, nu fick jag lite fart och drog ihop simuleringar på detta.
Det visade sig att min modell var fortfarande lite fel. Det ska inte vara någon bias-kompensering i A-matrisen utan de ska vara nollor och då får man detta resultat.
Gravitationskompensering fungerar som det ska även då jag bara satte det till 1 m/s^2.

De va en liten spännande grej att testa.
De va ett tag sedan gjorde ett filter från grunden upp se det va bra med lite repetition.

Edit:
En fin grej jag glömde säga. Man kan sampla sonarn ändå ner till 1Hz innan det börjar märkas lite på bias-kompenseringen.
De gillas skarpt! Samt den följer fortfarande höjden bra.

Höjd:

fig_height.png

Acceleration:

fig_acc.png

[Korken]

Här kommer simuleringsuppställningen också för de som vill testa själva.
Jag gör detta i Simulink i Matlab. Håll till godo!

sim.png

Kalman_itterate.m:

Kod: Markera allt

function x  = Kalman_itterate(z)
  % Paramaters:
  % P = State covariance matrix
  % x = State variables
  % z = Measurement
  % u = Input
  % n = Number of itterations
  
 
  persistent x_est P n;
  
  dt = 1/200;
  
  % Constants and model specific parameters
  % Double integrator
  A = [1, dt, dt^2/2, 0;
       0, 1, dt, 0;
       0, 0, 1, 0
       0, 0, 0, 1];
   
  B = [0, 0, 0, 0]';
  u = 0;
  
  % The H matrix maps the measurement to the states
  H = [1, 0, 0, 0;
       0, 0, 1, 1];
  
  % Tuning paramaters
  Q = [0.1, 0, 0, 0;
       0, 0.1, 0, 0;
       0, 0, 20, 0;
       0, 0, 0, 0.01];
  
  R = [1000, 0;
       0, 1000];
  
  % Startup conditions
  % P will converge after a few itterations
  if isempty(P)
    P = eye(length(Q))*10;
     
    x_est = [0, 0, 0, 1]';
    
    n = 1;
  else
    n = n+1;    % Good for measuring state covariance convergance
  end
  
  % ----------------------------
  %  Kalman Filter starts here!
  % ----------------------------
  
  % Model prediction
  x_est = A*x_est + B*u; % + w_k
  
  % State covariance matrix update based on model
  P = A*P*A' + Q;
  
  % Model and measurement differences
  y = z - H*x_est; % + v_k
  
  % Measurement covariance update
  S = H*P*H' + R;
  
  % Calculate Kalman gain
  K = P*H'/S;
  
  % Corrected model prediction
  x_est = x_est + K*y;      % Output state vector
  
  % Update state covariance with new knowledge
  I = eye(length(P));
  P = (I - K*H)*P;  % Output state covariance
  
  x = x_est;
  
  % ----------------------------
  %  Kalman Filter ends here!
  % ----------------------------

[Gimbal]

Hur bra funkar det i praktiken att dubbelintegrera värden från accelerometern?
Vill minnas att jag gjorde ett riktigt köksbordstest av detta för något år sedan med deprimerande resultat, hastighet och position vandrade iväg något hejdlöst. Inser ju att något filter måste användas men jobbade inte vidare med det då.

[Korken]

Det ska inte vara något problem.
Min dubbel integral "kalibreras" av sonarn så kommer den aldrig att sticka iväg. Jag har testat simuleringar med att ha den felkalibrerad, men filtret konvergerar på ca 20s i 200Hz (från G=0m/s^2 till 9.81m/s^2). Men jag har startvektorn på 9.8m/s^2 så filtret inte behöver spendera massa tid på att hitta det först.
Hur gjorde du? Hade du med graviatationsbias i ditt kalmanfilter?

[Gimbal]

Nja, jag använde en accelerometer som pekade längs med markplan och räknande på positionsändringar i sidled. Men som sagt, utan något som helst filter så räckte det med en mikroskopisk fel-signal för att hastighet och position skulle vandra i väg. Ej heller fanns någon annan signal att stötta sig mot så det var inget kalmanfilter utan enbart dubbelintegrering av signalen.

[LHelge]

Snyggt! Kommandot persistent i embedded MATLAB hade jag missat brukar lösa det med att skicka ut variabeln genom ett 1/z block och sen tillbaka igen. Det där var ju mycket snyggare.

[Korken]

Gimbal: Ah jag förstår! Jo, dubbelintegraler är per definition en väldigt instabil process (G(s)=1/s^2 -> -180 grader fasförskjutning) så man måste på något sätt kontrollera den så det inte finns ett litet fel som integreras över lång tid.
Jag löser det med att ha en fast referenspunk, men det finns säkerligen fler sätt att lösa det på!

LHelge: Tackar! Jag brukade också göra så men sedan ramlade jag över persistent i ett exempel om ett Kalmanfilter i Simulink i deras hjälp och såg då persistent.
Riktigt trevligt kommando! Samt så tycks mina system kompilera snabbare med det.

[Korken]

Nu har jag börjat fixa så den ska fungera med tryckmätaren också.
Det blir att ändra på R matrisen vid olika höjder så man koppar ur sonarn på för hög höjd, men jag återkommer snart med lite simuleringar!