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);
}
// invert
public Kvaternion invert()
{
Kvaternion F = new Kvaternion(new TVector(0, 0, 0), 2 * Math.Acos(w));
F.x = -x;
F.y = -y;
F.z = -z;
return(F);
}
public Kvaternion Minus(Kvaternion B)
{
Kvaternion C = new Kvaternion(new TVector(0, 0, 0), 2 * Math.Acos(w));
C.x = x - B.x;
C.y = y - B.y;
C.z = z - B.z;
C.w = w - B.w;
return(C);
}
public Kvaternion Plus(Kvaternion B)
{
Kvaternion C = new Kvaternion(new TVector(0, 0, 0), 2 * Math.Acos(w));
C.x = x + B.x;
C.y = y + B.y;
C.z = z + B.z;
C.w = w + B.w;
return(C);
}
public Kvaternion scale(double val)
{
Kvaternion V = new Kvaternion(new TVector(0, 0, 0),
2 * Math.Acos(w));
V.w = w * val;
V.x = x * val;
V.y = y * val;
V.z = z * val;
return(V);
}
// normalize
public Kvaternion normalize()
{
Kvaternion E = new Kvaternion(new TVector(0, 0, 0),
2 * Math.Acos(w));
E.x = x / Lenght();
E.y = y / Lenght();
E.z = z / Lenght();
E.w = w;
return(E);
}
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 void Fill(Kvaternion q)
{
mas[0, 0] = -2 * q[3];
mas[0, 1] = 2 * q[4];
mas[0, 2] = -2 * q[1];
mas[0, 3] = 2 * q[2];
mas[1, 0] = 2 * q[2];
mas[1, 1] = 2 * q[1];
mas[1, 2] = 2 * q[4];
mas[1, 3] = 2 * q[3];
mas[2, 1] = -4 * q[2];
mas[2, 2] = -4 * q[3];
}
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(); // Закрытие файла результата
}
internal void WriteAngles(StreamWriter sw)
{
//atan2 (2*(q1*q2+q3*q4),1-2*(q2*q2+q3*q3))
//asin (2*(q1*q3-q4*q2))
//atan2 (2*(q1*q4+q2*q3),1-2*(q3*q3+q4*q4))
Kvaternion T = this.normalize();
double a1 = Math.Atan2(2 * (T.x * T.y + T.z * T.w),
1 - 2 * (T.y * T.y + T.z * T.z));
double a2;
a2 = Math.Asin(2 * (T.x * T.z - T.w * T.y));
double a3 = Math.Atan2(2 * (T.x * T.w + T.y * T.z),
1 - 2 * (T.z * T.z + T.w * T.w));
sw.WriteLine(a1 + " " + a2 + " " + a3);
}
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));
}
public Sensor(TVector rotate, Double angle)
{
L = new Kvaternion(rotate, angle);
}
public void Fill(Kvaternion q, Kvaternion a)
{
x = 2 * (q[2] * q[4] - q[1] * q[3]) - a[2];
y = 2 * (q[1] * q[2] + q[3] * q[4]) - a[3];
z = 2 * (0.5 - q[2] * q[2] - q[3] * q[3]) - a[4];
}
