Dostrajanie filtru Kalmana (MPU6050)

[corsair]

Witam,

Jest to mój pierwszy post na forum, więc chciałbym się najpierw przywitać 🙂

Od jakiegoś czasu próbuję "dostroić" swój filtr Kalmana, którego zadaniem jest wyznaczenie odchylenia kątowego (dokonanie fuzji danych z akcelerometru i żyroskopu tak aby wyeliminować wpływ szumów akcelerometru). Moduł, którego używam to standardowy MPU6050 na płytce GY-521.

Algorytm filtru Kalmana wyznaczyłem najpierw na papierze krok po kroku, potem zapisując go w formie kodu w C:

Plik .h:

#ifndef KALMAN_H_
#define KALMAN_H_

#include <avr/io.h>

#define dt 0.020	 //czas próbkowania
#define r	0.08	// 0.25		//szum akcelerometru
#define q	0.05	// 0.1326	   //szum żyroskopu
#define q_angle 0.05  // 0.1

extern volatile  double omega;		//pomiar z żyroskopu w deg/sek
extern volatile  double z;			//pomiar z akcelerometru w g

extern volatile  double beta;		//kąt odchylenia
extern volatile  double sro;		//wartość szumu żyroskopu

extern volatile  double beta_;		//kąt odchylenia bez korekcji
extern volatile  double sro_;		//wartość szumu żyroskopu bez korekcji

extern volatile  double beta_prev;	//poprzednia wartość kąta
extern volatile  double sro_prev;	//poprzednia wartość szumu żyroskopu

extern volatile  double P11,P12,P21,P22;		//zmienne macierzy kowariancji
extern volatile  double P11_,P12_,P21_,P22_;	//zmienne macierzy kowariancji bez korekcji
extern volatile  double K1,K2;					//zmienne wzmocnienie kalmana (składowe macierzy wzmicnienia K)


void kalman_init();
void kalman_calculate_X(double omega_pom,double z_pom);		//wyznacz kąt obrotu wokół X
void kalman_calculate_X2(double omega_pom,double z_pom);	//inny algorytm kalmana
double get_X();												//zwróć obrót wokół X
double get_avg(int window);									//proste uśrednianie w oknie

#endif 

Plik .cpp

#include "kalman.h"

volatile double omega;//pomiar z żyroskopu w deg/sek
volatile double z; //pomiar z akcelerometru w g

volatile double beta;		//kąt odchylenia
volatile double sro;			//wartość szumu żyroskopu

volatile double beta_;		//kąt odchylenia bez korekcji
volatile double sro_;		//wartość szumu żyroskopu bez korekcji
volatile double beta_prev; //poprzednia wartość kąta
volatile double sro_prev; //poprzednia wartość szumu żyroskopu

volatile double P11,P12,P21,P22; //zmienne macierzy kowariancji
volatile double P11_,P12_,P21_,P22_; //zmienne macierzy kowariancji bez korekcji
volatile double K1,K2; //zmienne wzmocnienie kalmana (składowe macierzy wzmicnienia K)

void kalman_init()
{
beta=0;
beta_prev=0;

omega=0;
z=0;

sro=0.02;
sro_prev=0.02;

beta_=0;
sro_=0;

P11=0;
P12=0;
P21=0;
P22=0;

P11_=0;
P12_=0;
P21_=0;
P22_=0;

K1=0;
K2=0;
}

void kalman_calculate_X( double omega_pom,double z_pom )
{
beta_prev=beta;
sro_prev=sro;

//Faza predykcji
beta_=beta_prev+(omega_pom-sro_prev)*dt;
sro_=sro_prev;

P11_=P11-P21*dt+P22*dt*dt-P12*dt+q_angle;
P12_=P12-P22*dt;
P21_=P21-P22*dt;
P22_=P22+q;

//Faza korekcji
K1=P11_*(1/(P11_+r));
K2=P21_*(1/(P11_+r));

beta=beta_+K1*(z_pom-beta_);
sro=sro_+K2*(z_pom-beta_);

P11=P11_*(1-K1);		//P_00
P12=P12_*(1-K1);		//P_01
P21=P21_-(P11_*K2);		//P_10
P22=P22_-(K2*P12_);		//P_11
}

void kalman_calculate_X2( double omega_pom,double z_pom )
{
double y=0;
double S=0;

 beta += dt * (omega_pom - sro);
 P11 +=  - dt * (P21 + P12) + q_angle * dt;
 P12 +=  - dt * P22;
 P21 +=  - dt * P22;
 P22 +=  + q * dt;

 y = z_pom - beta;
 S = P11 + r;
 K1 = P11 / S;
 K2 = P21 / S;

 beta +=  K1 * y;
 sro  +=  K2 * y;
 P11 -= K1 * P11;
 P12 -= K1 * P12;
 P21 -= K2 * P11;
 P22 -= K2 * P12;
}

double get_X()
{
return beta;
}

double get_avg( int window )
{
double mean=0;

}

Dobierałem bardzo różne parametry filtra dt, r, q i q_angle i najlepsze wyniki jakie udało mi się uzyskać zamieściłem na wykresach. Na czarno jest odchylenie kątowe z akcelerometru, na czerwono z filtru Kalmana a na niebiesko proste całkowanie prędkości kątowej z uwzględnieniem dryfu.

1. Czujnik w spoczynku:

2. Czujnik obracany w ręce:

Z początku myślałem, że nieudolnie policzyłem równania, lub gdzieś się pomyliłem, jednak znalazłem kilka innych algorytmów w internecie, które jak twierdzą autorzy "na pewno działają" a wyniki są niemal identyczne. Z wykresów widać, że w stanie spoczynku szumy pomiaru akcelerometru i filtru Kalmana niewiele się od siebie różnią (jedynie filtr wprowadza zgodnie z teorią pewne opóźnienie). Przy niektórych parametrach filtru uzyskałem nieco różniące się odchylenia standardowe(w stopniach):

0,082 dla pomiaru z akcelerometru

0,063 dla pomiaru z filtru Kalmana

Odchylenie kątowe wyznaczone z samego akcelerometru wyliczam ze wzoru:

a=sqrt(abs(azg*azg+ayg*ayg-1));

beta_a=atan(ayg/azg)-atan(a/9.81);

beta_a=beta_a*180/3.14;

Czas próbkowania dla filtru wynosi 0.020s (50Hz), MPU6050 próbkuje z częstotliwością 200Hz. Zakres pomiarowy akcelerometru to +-2g, a żyroskopu +-2000 deg/s, Atmega 328 pracuje z częstotliwością 16MHz. Stosuję dużo elementów filtrujących oraz zasilam układ z stabilizowanego zasilacza aby wyeliminować jak najwięcej zakłóceń.

Czy wiecie co może być problemem w mojej implementacji tego filtru? Czy być może on już działa poprawnie i wiele więcej nie da się zrobić? Zastanawiam się jeszcze nad jakimś prostym filtrem FIR, choć nie chciałbym wprowadzać dodatkowych opóźnień. Czy można uzyskać wyniki takie jak na tym filmiku na yt?:

[corsair]

Jakiś czas temu udało mi się naprawić błąd - polegał na tym, że przez bałagan w kodzie filtr Kalmana działał co 100 ms zamiast 20 ms. Teraz wszystko pięknie śmiga 🙂