Niedziałający filtr Kalmana, Arduino + MPU 6050

[explosion95]

Cześć

Na swojej płytce Arduino UNO wraz z dołączonym modułem MPU-6050 zaimplementowałem ostatnio filtr Kalmana wyliczający odchylenie kątowe kąta Pitch, w sumie wyszło pewnie identycznie do jednego z przykładów zamieszczonych w publikacji pana Kędzierskiego z PWR. Problem w tym, że filtr nie działa całkowicie poprawnie a nie umiem znaleźć przyczyny. Poniżej pozwolę sobie wkleić całość kodu, który zawiera m.in. dokładnie opisane modele we/wy, macierze itd, a prócz tego jakieś zrzuty ekranu z tego co dzieje się obliczonym w FK kątem. Przepraszam, że kod jest długi, ale jest chyba całkiem znośnie skomentowany, filtr znajduje się mniej więcej od połowy w dół, a zamieściłem cały kod gdyż może błąd jest gdzieś poza filtrem? Tylko że filtr komplementarny, który też tam mam działa poprawnie...

/*
*Kod pobierania danych z modulu MPU-6050 bez uzycia zewnetrznych bibliotek
*Jedyna stosowana biblioteka to <Wire.h> do komunikacji I2C z modułem pomiarowym
*
*Zakres pomiarowy zyroskopu ustawiam chyba na +/- 500deg/s
*Zakres pomiarowy akcelerometru ustawiam chyba na +/-4g
*
*Moduł MPU-6050 pozwala też na pomiar i odczytywanie temperatury
*Wartosci są schowane w rejestrach 0x41 i 0x42
*
*Wszelkie dane odnosnie dzialania i inicjowania pracy modulu, mapy rejestrow dostepne
*są w nocie katalogowej MPU-6050
*/


                          ////////////////////////
                          // POCZATEK PROGRAMU  //
                          ////////////////////////


#include <Wire.h>

int kalibracja = 0;                              // zmienna potrzebna do kalibracji zyroskopu
float deltaT=0;                                  // wazna zmienna, definiuje czas miedzy pomiarami
int zliczanie =0;
long raw_acc_X, raw_acc_Y, raw_acc_Z;            // surowe dane z rejestrow akcelerometru   
long sum_gyro_X, sum_gyro_Y, sum_gyro_Z;         // zmienne do sumowania odczytow z zyroskopu

//long loop_timer;                               // do mierzenia czasu trwania petli
long acc_vector;                                 // zmienna do liczenia Pitagorasa z skladowych akcelerometru
unsigned long czas1, czas2 =0;                   // do mierzenia czasu trwania petli i liczenia wartosci deltaT

int raw_gyro_X, raw_gyro_Y, raw_gyro_Z;          // surowe dane z rejestrow gyroskopu

float gyro_pitch, gyro_roll, gyro_yaw;           // zmienne na katy wyliczone z pomiarow zyroskopu
float acc_pitch, acc_roll, acc_yaw;              // zmienne na katy wyliczone z pomiarow akcelerometru
float output_pitch, output_roll, output_yaw;     // zmienne przechowujace ostatecznie wyliczone katy

float pitch_spirit_level = 2.5;                  // 1.34SPRAWDZ odczyty z acc na poziomicy
float roll_spirit_level = -3.0;                  //-4.33 i wpisz do tych zmiennych!!!!

boolean first_cycle = true;                      // przy pierwszym wzieciu danych kat zyroskopu jest
                                                // kątem akcelerometru
double liczba = 0.000000000026645;  

////////////////////////////////
// ZMIENNE DO FILTRU KALMANA  //
//                            /////////////////////////////////////////////////////////////////////////////////////
   float kalman_pitch = 0;
    float acc_pitch1, gyro_drift2, gyro_drift1 = 0;  // wyzerowane zmienne dla pierwszej petli obliczen

   float P1_11, P1_22 = 1;                                      // elementy macierzy kowariancji P a posteriori
   float P1_12, P1_21 = 0;

   float Q_11, Q_22 = 0.0001;                                   // elementy macierzy kowariancji szumu procesowego
    float Q_12, Q_21 = 0;                   

   float A_11, A_T_12 = 1;                                      //elementy macierzy wejścia A oraz macierzy transponowanej wejścia A^T
    float A_12, A_T_11 = -deltaT;
    float A_21, A_T_22 = 0;
   float A_22, A_T_21 = 1;

    float P2_11, P2_12, P2_21, P2_22 = 0;                        // elementy macierzy kowariancji P a priori

    float R = 10;                                                //kowariancja szumu pomiarowego

    float H_1, H_T_2 = 1;                                        //elementy macierzy przejścia H i macierzy transponowanej przejścia H^T
    float H_2, H_T_1 = 0;

    float K_1, K_2 = 0;                                          //wektor wzmocnienia Kalmana, jest 2x1
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


                         /////////////////////////
                         //  PETLA JEDNORAZOWA  //
                         /////////////////////////


void setup() 
{
Serial.begin(115200);                      // rozpoczecie komunikacji szeregowej
Wire.begin();                              // zainicjowanie komunikacji Wire
SetupMPU();                                // zainicjowanie stanu MPU przed praca
GyroCalibration();                         // kalibracja zyroskopu, usrednienie 2000 wynikow
}



                         ////////////////////////
                         //    PETLA GLOWNA    //
                         ////////////////////////


void loop() 
{
 zliczanie = zliczanie +1;
     czas1=micros(); 

 RecordGyroRegisters();                                     // pobierz dane z zyroskopu

        raw_gyro_X=raw_gyro_X - sum_gyro_X;                       // odejmij od pobranych danych wartosci
        raw_gyro_Y=raw_gyro_Y - sum_gyro_Y;                       // obliczone przy kalibracji
        raw_gyro_Z=raw_gyro_Z - sum_gyro_Z;

 ProcessGyroData();                                         // przelicz dane z zyroskopu
                                                            // na cos wartosciowego                                  
 RecordAccelRegisters();

 ProcessAccelData();    

 ComplementaryFilter();

 KalmanFilter();


if(zliczanie == 5)
{
 PrintData();
zliczanie = 0;
}

     czas2=micros();                      // wazna linijka, tu sie oblicza 
     deltaT=czas2-czas1;                  // czas wykonania petli sterujacej

}


                         ////////////////////////
                         //    KONIEC PETLI    //
                         ////////////////////////




void SetupMPU()                             // funkcja ustawienia wartosci poczatkowych w MPU
{
 Wire.beginTransmission(0b1101000);        // rozpoczecie transmisji, inaczej 0x68, czyli adres MPU
 Wire.write(0x6B);                         // ustawiamy dostęp do rejestru 6B by ustawić tryb pracy
 Wire.write(0b00000000);                   // rejestr czuwania wylaczony itp
 Wire.endTransmission();                   // zakonczenie transmisji

 Wire.beginTransmission(0b1101000);        // rozpoczecie transmisji, inaczej 0x68, czyli adres MPU
 Wire.write(0x1B);                         // rejestr 1B do konfiguracji pracy zyroskopu
 Wire.write(0b00001000);                   // tryb pracy do +/- 500 deg/s
 Wire.endTransmission();                   // zakonczenie transmisji

 Wire.beginTransmission(0b1101000);        // rozpoczecie transmisji, inaczej 0x68, czyli adres MPU
 Wire.write(0x1C);                         // rejest 1C do konfiguracji pracy akcelerometru
 Wire.write(0b00001000);                   // tryb pracy do +/- 4g
 Wire.endTransmission();                   // zakonczenie transmisji
}


void GyroCalibration()
{
   for (int kalibracja = 0; kalibracja < 1000 ; kalibracja ++)
   {               
   RecordGyroRegisters();                  // w każdej petli odczytaj wartosci z zyroskopu
   sum_gyro_X = sum_gyro_X+raw_gyro_X;     // w kazdej petli w zmiennej sum_gyro_X sumuj kolejne pomiary
   sum_gyro_Y = raw_gyro_Y+sum_gyro_Y;     // w kazdej petli w zmiennej sum_gyro_Y sumuj kolejne pomiary
   sum_gyro_Z = raw_gyro_Z+sum_gyro_Z;     // w kazdej petli w zmiennej sum_gyro_Z sumuj kolejne pomiary
   delay(3);                               // Delay 3us to simulate the 250Hz program loop
   }
 sum_gyro_X = sum_gyro_X/1000;             // uśrednij wyniki pomiarow i zapisz do sum_gyro_X
 sum_gyro_Y = sum_gyro_Y/1000;             // uśrednij wyniki pomiarow i zapisz do sum_gyro_X
 sum_gyro_Z = sum_gyro_Z/1000;             // uśrednij wyniki pomiarow i zapisz do sum_gyro_X
}


void RecordAccelRegisters()
{
 Wire.beginTransmission(0b1101000);        // rozpoczecie transmisji, inaczej 0x68, czyli adres MPU
 Wire.write(0x3B);                         // ustawiam rejest 0x3B do danych akcelerometru
 Wire.endTransmission();                   // zakonczenie transmisji
 Wire.requestFrom(0b1101000,6);            // od 0x3B ma sie zaczac pobieranie 6 bajtow danych o acc
 while(Wire.available()<6);
 raw_acc_X=Wire.read()<<8|Wire.read();     // dane acc_x/y/z przechowywane sa w 8bitowych rejestrach
 raw_acc_Y=Wire.read()<<8|Wire.read();     // od adresu 0x3B do 0x40 wlacznie i wlasnie je pobieramy
 raw_acc_Z=Wire.read()<<8|Wire.read();     // i zapisujemy do zmiennych, na kazda os 2 bajty danych

}


void ProcessAccelData()
{                                                               //z Pitagorasa przeliczam dlugosc calkowita wektora sily ciezkosci
 acc_vector = sqrt((raw_acc_X*raw_acc_X)+(raw_acc_Y*raw_acc_Y)+(raw_acc_Z*raw_acc_Z));
 acc_pitch = asin((float)raw_acc_Y/acc_vector)* 57.296;
 acc_roll = asin((float)raw_acc_X/acc_vector)* -57.296;        // wzory obok sa by obliczyc kąty pomiedzy skladowymi sil
                                                               // a te katy to jednoczesnie kąty obrotu modulu
 acc_pitch = acc_pitch - pitch_spirit_level;
 acc_roll = acc_roll - roll_spirit_level; 
}


void RecordGyroRegisters()
{
 Wire.beginTransmission(0b1101000);        // rozpoczecie transmisji, inaczej 0x68, czyli adres MPU
 Wire.write(0x43);                         // ustawiam rejest 0x43 do danych zyroskopu
 Wire.endTransmission();                   // zakonczenie transmisji
 Wire.requestFrom(0b1101000,6);            // od 0x3B ma sie zaczac pobieranie 6 bajtow danych o acc
 while(Wire.available()<6);
 raw_gyro_X=Wire.read()<<8|Wire.read();    // dane gyro_x/y/z przechowywane sa w 8bitowych rejestrach
 raw_gyro_Y=Wire.read()<<8|Wire.read();    // od adresu 0x43 do 0x48 wlacznie i wlasnie je pobieramy
 raw_gyro_Z=Wire.read()<<8|Wire.read();    // i zapisujemy do zmiennych, na kazda os 2 bajty danych
}


void ProcessGyroData()                                                    // deltaT to czas probkowania ~2500us = 2.5ms = 0.0025s
{             
                                                                         // 65.5 to wartosc dla +/-500deg/s wzieta z dokumentacji
 gyro_pitch=gyro_pitch+(raw_gyro_X * (deltaT/65500000));                 // robimy calkowanie by dostac wartosc kąta deg             //sprawdz wzory uwzgledniajac deltaT!!!!
 gyro_roll=gyro_roll+(raw_gyro_Y * (deltaT/65500000));                   // 
 gyro_yaw=gyro_yaw+(raw_gyro_Z * (deltaT/65500000));                     // wyniki bedace wychyleniem katowym z pomiarow zyroskopu

gyro_pitch=gyro_pitch+gyro_roll*sin(raw_gyro_Z*deltaT*liczba);           //          
gyro_roll=gyro_roll-gyro_pitch*sin(raw_gyro_Z*deltaT*liczba);            // tymi obliczeniami likwidujemy gimbal lock (nieprawda ale skrot myslowy)
}

void ComplementaryFilter()
{
 if (first_cycle==true)            // warunek spełniony wylacznie raz          // to wczesniej bylo w funkcji ComplementaryFilter
 {                                 // w pierwszym obiegu sterowania            // na samym poczatku
   gyro_pitch = acc_pitch;         // zeby wpisac do gyro polozenie poczatkowe
   gyro_roll = acc_roll;           // z pomiarow akcelerometru
   first_cycle = false;            

   output_pitch = gyro_pitch;      // w pierwszej petli wpisujemy to
   output_roll = gyro_roll;        // bezposrednio na wyjscie 
 }

// FILTR KOMPLEMENTARNY DLA KĄTÓW PITCH I ROLL

     output_pitch = 0.97 * output_pitch  + 0.03 * acc_pitch;
     output_roll = 0.97 *output_roll + 0.03 * acc_roll;
     output_yaw = gyro_yaw;
}


// FILTR KALMANA DLA ESTYMACJI KĄTA PITCH

void KalmanFilter()
{

 //////////////////////////////////////////////////////////////////////////
 //     OPIS GŁÓWNY RÓWNAN I MACIERZY UZYTYCH DO FILTRU KALMANA          //
 //  FILTR TEN MA ZA ZADANIE ESTYMOWAC KĄT OBROTU PITCH NA PODSTAWIE     //
 //             WSKAZAN AKCELEROMETRU I ZYROSKOPU MPU 6050               /////////////////////////////////////////////////////////////////////////////////////
 //
 // Adnotacja i komentarze: starałem sie jak moglem wszystko opisać ale nie da sie, ponizej znajdują sie wzory wykorzystywane na kazdym etapie
 // obliczen ale takze wymnażane recznie "na piechotę" przez siebie poszczególne elementy różnych macierzy. To dlatego bo uC nie ma
 // zaimplementowanych bibliotek do liczenia macierzy wiec trzeba to robić recznie
 // jakby cos, to wszystko robilem na kartce, i tam tez upraszczalem, stad niektore wzory (np na elementy macierzy kowariancji P2) 
 // moga byc niejasne, bo po prostu są juz uproszczone, wynikają mimo to z poprawnych (oby) przeksztalcen wedlug zasad działan na macierzach
 //
 //
 //                        x2 = A*x1 + B*u + w;       // w to szum procesowy
 //                        z = H*x2 + v;              // x2 to stan x a priori, A i B to macierze (wejscia i sterowania), u to wektor sterowania, x1 to stan a posteriori
 //                                                   // v to szum pomiarowy, H to macierz przejscia (wyjścia) z procesu do pomiaru, z to pomiar 
 //
 //                                                   // x to stan, macierz [acc_pitch2; gyro_drift2] stosujemy trick ze dryft jest nasza zmienna stanu
 //
 //         //RÓWNANIA NASZEGO STANU
 //
 //         kalman_pitch = acc_pitch1 +gyro_pitch*deltaT - gyro_drift1*deltaT;  // rownanie estymowanego kąta obrotu
 //         gyro_drift2 = gyro_drift1;                                          // zakladamy ze dryft jest staly
 //                                      
 //                     x = [kalman_pitch; gyro_drift2]                         // to jest stan naszego systemu  
 //                                                                             // dany wektorem z dwiema zmiennymi stanu
 //                                                                             // czyli kątem i dryftem
 //
 //                     u = gyro_pitch                                          // predkosc katowa z zyroskopu jest 
 //                                                                             // wektorem sterujacym układem
 //
 //                     A = | 1 -deltaT |                                       // macierz wejścia A
 //                         | 0       1 |
 //
 //
 //                     B = | deltaT |                                          // macierz sterowania B
 //                         |    0   |
 //
 //                     H = | 1   0 |                                           // macierz wyjścia H
 //
 //                     R = | 1 |                                               // macierz kowariancji szumu pomiarowego
 //
 //                     Q = | 1    0 |*q                                        // Q to macierz kowariancji szumu procesowego
 //                         | 0    1 |                                          // q doświadczalnie 0.0001
 //
 //                     P = | P_11  P_12 |                                      // macierz kowariancji 
 //                         | P_21  P_22 |  
 //
 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

 ///////////////////////////////////////////////
 //               ETAP PREDYKCJI              //
 //                                           ///////////////////////////////////////////////////////////////////////////////////////////
 //
 //            x2 = A * x1 + B * u;              //predykcja nastepnego stanu systemu na podstawie poprzedniej estymaty
 //            P2 = A * P1 * A^T + Q;            //predykcja nowej macierzy kowariancji na podstawie poprzedniej 
 //                                              //macierzy kowariancji szumu procesowego Q
 //        



                 // RÓWNANIE PREDYKCJI NA POLOZENIE KATOWE WOKOL OSI X (PITCH)

              kalman_pitch = acc_pitch1 +raw_gyro_X*deltaT/65.5 - gyro_drift1*deltaT;   // predykcja nowego kąta pitch na podstawie poprzedniego
              gyro_drift2 = gyro_drift1;                                                // oraz predkosci pochodzącej z zyroskopu
                                                                                        // raw_gyro_X/65.5 to predkosc obrotowa w deg/s wg noty
 //                                                                                     // katalogowej MPU 6050
 //           P1_11 = 1;                 Q_11 = 0.0001;
 //           P1_12 = 0;                 Q_12 = 0;
 //           P1_21 = 0;                 Q_21 = 0;
 //           P1_22 = 1;                 Q_22 = 0.0001;
 //
 //           A_11 = 1;                  A_T_11 = -deltaT;
 //           A_12 = -deltaT;            A_T_12 = 1;
 //           A_21 = 0;                  A_T_21 = 1;
 //           A_22 = 1;                  A_T_22 = 0;
 //
 //         // RÓWNANIE PREDYKCJI NA NOWA MACIERZ KOWARIANCJI (rozpisane na kartce a poniżej juz uproszczone)
 //   P2_11 = ((A_11*P1_11 + A_12*P1_21)*A_T_11 + (A_11*P1_12 + A_12*P1_22)*A_T_21) + Q_11; 
 //   P2_12 = ((A_11*P1_11 + A_12*P1_21)*A_T_12 + (A_11*P1_12 + A_12*P1_22)*A_T_22) + Q_12;
 //   P2_21 = ((A_21*P1_11 + A_22*P1_21)*A_T_11 + (A_21*P1_12 + A_22*P1_22)*A_T_21) + Q_21;
 //   P2_22 = ((A_21*P1_11 + A_22*P1_21)*A_T_12 + (A_21*P1_12 + A_22*P1_22)*A_T_22) + Q_22;


                         // ELEMENTY MACIERZY KOWARIANCJI PO ETAPIE PREDYKCJI

             P2_11 = ((P1_11 + (A_12*P1_21))*A_T_11 + (P1_12 + (A_12*P1_22))) + Q_11; 
             P2_12 = P1_11 + (A_12*P1_21);
             P2_21 = P1_21*A_T_11 + P1_22;
             P2_22 = P1_21 + Q_22;

 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 //
 //////////////////////////////////////////////////////////////////////
 //                                                                  //
 //                       ETAP KOREKCJI                              //
 //                                                                  //////////////////////////////////////////////
 //
 //               // OBLICZENIA WZMOCNIENIA KALMANA
 //
 //           K = P2 * H^T * (H * P2 * H^T + R)^1                        // wzór na okreslenie wzmocnienia Kalmana dla 
 //          x2 = x2 + K*(z - H*x2)                                      // obecnej iteracji
 //          P2 = ( I - K*H) *P2                                         // z to acc_pitch czyli odczyt polozenia z acc
 //
 //
 // 
             K_1 = P2_12 * (1/(P2_12+R));                                // juz uproszczone wzory na poszczegolne elementy
             K_2 = P2_22 * (1/(P2_12+R));                                // macierzy wzmocnienia Kalmana


                  // ETAP KOREKCJI I RÓWNANIA KOREKCYJNE DLA STANU UKŁADU

               kalman_pitch = kalman_pitch + K_1*(acc_pitch-kalman_pitch); // etap korekcji i okreslenie nowego stanu
               gyro_drift2 = gyro_drift2 + K_2*(acc_pitch-kalman_pitch);   // wedlug oficjalnych wzorów dla filtru Kalmana

                  // ETAP KOREKCJI I RÓWNANIE KOREKCYJNE MACIERZY KOWARIANCJI

               P2_21 = K_2*P2_11 - P2_21;
               P2_11 = (K_1 - 1)*P2_11;
               P2_12 = (K_1 - 1)*P2_12;
               P2_22 = -P2_22;

                  // NA KONIEC AKTUALIZACJA ZMIENNYCH I ZAMKNIECIE PETLI

               P1_11 = P2_11;
               P1_12 = P2_12;                                            // nasza najnowsza macierz kowariancji
               P1_21 = P2_21;                                            // staje się poprzednia i idzie do
               P1_22 = P2_22;                                            // nowej petli obliczen

               acc_pitch1 = kalman_pitch;                                // nasz najnowszy najlepszy stan
               gyro_drift1 = gyro_drift2;                                // staje sie poprzednim stanem
                                                                         // i trafia do nowej petli obliczeń


}
//KONIEC FILTRU KALMANA


void PrintData()
{                                           // funkcja do wyswietlania wartosci na monitor


 Serial.print(kalman_pitch);
 Serial.println();

}

Nie rozumiem powodu dla którego filtr oblicza końcowy wynik na tak ogromną liczbę, która nie mieściła się w zmiennej float:

Natomiast po zmianie wszystkich zmiennych używanych w filtrze na zmienne typu long otrzymałem wykresy:

Skąd wzięły się tak duże wartości kąta? Jak widać na powyższym rysunku, charakter zmian kąta jest zgodny z ruchem modułu, tylko wartości są rzędu 10^7, a ponadto na ostatnim zdjęciu widać, że w bezruchu filtr wykazywał stałe płynięcie i wzrost wartości kąta.

Czy jest szansa, by zechciał ktoś zerknąć..? Skoro wartości zmieniają się zgodnie z ruchem modułu, to może rozwiązanie jest proste? Czy może być to kwestia źle ustawionych wartości początkowych? Albo źle wpisanych wartości do macierzy szumów Q i R?

[Lukaszm]

Bezpośrednio Twojego problemu nie rozwiążę, ale mogę podpowiedzieć jak ja bym podszedł do tematu będąc w Twojej sytuacji.

Na pewno nie robiłbym pierwszej implementacji na Arduino. Nie masz możliwości łatwego wyświetlania danych i rejestrowania ich. Nie możesz zatrzymać programu w trakcie działania, żeby podejrzeć sobie wartości zmiennych. Działanie trochę po omacku w mojej opinii.

Ja przetestowałbym funkcję KalmanFilter() na komputerze. Zakładasz sobie jakąś tam zmianę rzeczywistego kąta estymowanego (generalnie to stanu systemu, bo przyspieszenie też uwzględniasz) (tego filtr nie zna, to jest Twoje ground truth względem którego będziesz oceniał działanie filtru). Na podstawie stanu rzeczywistego wyliczasz, jakie byłyby idealne pomiary (z żyroskopu i akcelerometru), możesz sobie do tych danych dodać szum ewentualnie. Dane te wprowadzasz do Twojej funkcji KalmanFilter(), estymujesz i tworzysz wykres z kątem rzeczywistym i estymowanym. Możesz sobie przetestować jak warunki początkowe wpływają na działanie, jak macierze Q i R itd.

Jak to działa, to sprawdzasz na Arduino czy na pewno Twoje pomiary są poprawne (jednostki, zwroty itd.). Wtedy dopiero uruchamiasz cały system i powinno być ok.

Oczywiście możesz nadal działać wyłącznie na Arduino i sprawdzać, co się stanie jak zmienisz float na long i przy każdym teście ruszać rzeczywistym systemem, ale sensowniejsze wydaje mi się opisane przeze mnie podejście.

[explosion95]

Lukaszm, bardzo dziękuję i za odpowiedź, i za propozycje!

Jednakże zanim do nich przystąpiłem, wydaje mi się, że odnalazłem potencjalne źródło błędów! A przyczyna (jeśli to ta) okazała się taka prosta... w wzorach używanych w filtrze przeliczam wszystko z użyciem wartości deltaT, tylko że mam ją w mikrosekundach... stąd możliwe że wartości są akurat 10^6 większe niż poprawne. Po poprawce chyba jest w porządku

W tym momencie chciałbym spytać, jako że w ogóle nie mam wyczucia i doświadczenia, czy są jakieś ogólne wskazówki co do oszacowania lub dobrania wartości macierzy kowariancji szumów Q i R (albo według innych zródeł V i W)? Czy R powinno być duże i czy Q powinno mieć wartości tylko na przekątnej? Czy wartości na przekątnej Q mają być takie same czy różne? Spotkałem się z wieloma sposobami

Pytam i proszę gdyż gdyż filtr zachowuje się bardzo dziwnie, wpada w sinusoidalne oscylacje o zmieniającej się samoistnie częstotliwości. Na obecną chwilę nie potrafię określić reguły jaka odpowiada za takie zachowanie gdy zmieniam parametry szumów..