public TVector Povorot(TVector V)
{
Kvaternion T = this.mult(V);
T = T.mult(this.invert());
return(new TVector(T.x, T.y, T.z));
}
public Sensor(string Filename)
{
TVector A = new TVector(1, 1, 1);
L = new Kvaternion(A, Math.PI / 2);
myFile = Filename;
sr = File.OpenText(myFile);
}
public Kvaternion(TVector rotate, Double angle)
{
TVector temp = rotate.normal();
w = Math.Cos(angle / 2);
x = temp.x * Math.Sin(angle / 2);
y = temp.y * Math.Sin(angle / 2);
z = temp.z * Math.Sin(angle / 2);
sw = null;
}
public Kvaternion mult(TVector b)
{
Kvaternion V = new Kvaternion(new TVector(0, 0, 0),
2 * Math.Acos(w));
V.w = -x * b.x - y * b.y - z * b.z;
V.x = w * b.x + y * b.z - z * b.y;
V.y = w * b.y - x * b.z + z * b.x;
V.z = w * b.z + x * b.y - y * b.x;
return(V);
}
public Kvaternion mult(TVector b)
{
Kvaternion C = new Kvaternion(new TVector(1, 1, 1),
Math.PI / 2);
//C.x = mas[0, 3] * b.x + mas[1, 3] * b.y + mas[2, 3] * b.z;
C.setx(mas[0, 0] * b.x + mas[0, 1] * b.y + mas[0, 2] * b.z);
//C.y = mas[0, 2] * b.x + mas[1, 2] * b.y + mas[2, 2] * b.z;
C.sety(mas[1, 0] * b.x + mas[1, 1] * b.y + mas[1, 2] * b.z);
C.setz(mas[2, 0] * b.x + mas[2, 1] * b.y + mas[2, 2] * b.z);
C.setw(mas[3, 0] * b.x + mas[3, 1] * b.y + mas[3, 2] * b.z);
return(C);
}
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 Sensor(TVector rotate, Double angle)
{
L = new Kvaternion(rotate, angle);
}
public Accel(TVector rotate, Double angle)
: base(rotate, angle)
{
}
public Gyro(TVector rotate, Double angle)
: base(rotate, angle)
{
}
