Filtracja Kalmana

[mrufka3]

Przede wszystkim chciałem się przywitać, gdyż to mój pierwszy temat/post na tym forum (a już potrzebuję pomocy, aż mi kurcze głupio :/) Słuchajcie jest taka sprawa: przy filtracji Kalmana mamy we wzorach taką macierz P definiowaną jako (H transponowane * H)^-1. Więc dla dwóch czujek wygląda ona mniej więcej tak (każdy element liczony osobno ze względu na to, że to stosunkowo małe macierze i nie ma potrzeby pisać jakiejś struktury w ansi C):

P[0][0] -= P[1][0] - P[0][1] + P[1][1] + Q;

P[0][1] -= P[1][1];

P[1][0] -= P[1][1];

P[1][1] += Q;

gdzie Q to ustalona doświadczalnie wariancja predykcji. No i tu jest mój problem, bo mam napisać tą macierz (ogólnie całą filtrację, ale z tą macierzą mam problem) jeszcze dla 3 i 4 czujek. Bardzo proszę o pomoc.