public TVector Povorot(TVector V)
{
Kvaternion T = this.mult(V);
T = T.mult(this.invert());
return(new TVector(T.x, T.y, T.z));
}
static void Main(string[] args)
{
Sensor Gyro = new Sensor("gyro.csv"); // Cоздаем датчик гироскопа
Sensor Accel = new Sensor("accel.csv"); // Создаем акселерометр
// Летательный аппарат
Kvaternion L = new Kvaternion(new TVector(1, 1, 1), Math.PI / 2);
StreamWriter sw = File.CreateText("Kvaternion.txt");
Double t0 = 0, tk = 0.8, dt = 0.01;
Double Beta = 1;
Double T = t0;
while (T < tk)
{
// Взять показания гироскопа
Kvaternion G = Gyro.getKvaternion().normalize();
// Взять показания акселерометра
Kvaternion A = Accel.getKvaternion().normalize();
//Угловые скорости
Kvaternion S = L.mult(G).scale(0.5);
TVector K = new TVector(0, 0, 0);
K.Fill(G, A);
Matr J = new Matr(3, 4);
J.Fill(G);
// Кватернион градиент F
Kvaternion W = J.Trans().mult(K).normalize();
// Шаг алгоритма
S = S.Minus(W.scale(Beta));
L = L.Plus(S.scale(dt));
Console.WriteLine("G=" + G.ToString());
Console.WriteLine("L=" + L.ToString());
Console.WriteLine("S=" + S.ToString());
// Сохранение результатов и переход на следующий шаг
// L.Write(sw);
L.WriteAngles(sw);
T = T + dt;
}
sw.Close(); // Закрытие файла результата
}
public Kvaternion quat_from_angles_rad()
{
Kvaternion q_h = new Kvaternion(new TVector(0, 0, 0), 0);
q_h.setz(Math.Sin(heading / 2));
q_h.setw(Math.Cos(heading / 2));
Kvaternion q_a = new Kvaternion(new TVector(0, 0, 0), 0);
q_a.sety(Math.Sin(altitude / 2));
q_a.setw(Math.Cos(altitude / 2));
Kvaternion q_b = new Kvaternion(new TVector(0, 0, 0), 0);
q_b.setx(Math.Sin(bank / 2));
q_b.setw(Math.Cos(bank / 2));
Kvaternion q_temp = q_h.mult(q_a);
return(q_temp.mult(q_b));
}
