Matematické Fórum

Dobrý den
Řeším jeden problém s rotačními maticemi.
Mám matici M, která mi popisuje jak je R/C model natočen vúči počáteční pozici (když se připojí řídící jednotka k napájení, resp = cca země). Tato matice je měněna v každém cyklu programu za pomocí rotačních matic, kde vstup dat tvoří přesný MEMS senzor.

Pak mám matici C, která mi popisuje chtěnou pozici modelu (jak bych chtěl, aby byl model ve vzduchu úhlově natočen), tuto matici ovlivňuje pouze to, co dělá pilot na páčkách vysílače.

Mám tedy dvě matice, které integrují dva ruzné vstupy za pomocí rotačních matic a mám tak dvě Direction Cosine Matrices.

Můj problém spočívá v tom, že potřebuji hodnoty, je jedno jakých jednotek a ty hodnoty mi mají udávat odchylku matice M od matice C. Tato odchylka má jednoduše řečeno udávat jak moc je model odkloněn od chtěné pozice, resp. úhlu natočení modelu, z matice C. Pomocí této odchylky a regulace pak za pomocí serv na modelu ho budu schopen vyrovnat do té pozice, jak si přeje pilot. Chci z těchto matic prostě dostat odchylku od setpointu (matice C).

Abych dostal úhel natočení z obou DCM pro osu X, Y a Z, využívám funkce atan2 a asin. Jsou to matice 3x3, v každém cyklu se pro zmenšení chyby normalizuji, vše potřebné o DCM lze zjistit asi tady: http://www.starlino.com/dcm_tutorial.html
O rotačních maticích zase zde: http://en.wikipedia.org/wiki/Rotation_matrix

Když obě matice mezi sebou vynásobím, tak sice to funguje, ale jen za předpokladu, že je model v počáteční pozici. Když model např. otočím vzhúru nohama (provedu rotaci kolem osy Y o 180°), i když jsem tak také nasměroval matici C, tak poté to co dostávám je, že tato získaná odchylka reaguje obráceně, než správně má.
Takže potřebuji, aby to bylo nezávislé na pozici vuči zemi, ale závislé vůči modelu, tedy násobení těchto matic není správné řešení.

Uměl by mi někdo poradit?
Velice děkuji