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) { }