/// <summary>
/// This is called when the game should draw itself.
/// </summary>
/// <param name="gameTime">
/// Provides a snapshot of timing values.</param>
protected override void Draw(GameTime gameTime)
{
graphics.GraphicsDevice.Clear(Color.CornflowerBlue);
// Draw the background.
// Start building the sprite.
spriteBatch.Begin(SpriteBlendMode.AlphaBlend);
// Draw the background.
spriteBatch.Draw(background, mainFrame, Color.White);
// End building the sprite.
spriteBatch.End();
cubeEffect.Begin();
if (algorithm == 0) //Complementary filter
{
double[] anglesFiltered = new double[3];
anglesFiltered[0] = filter.getFilteredAngles()[0, 0];
anglesFiltered[1] = filter.getFilteredAngles()[1, 0];
anglesFiltered[2] = filter.getFilteredAngles()[2, 0];
MatrixLibrary.Matrix anglesMatrix = new MatrixLibrary.Matrix(3, 1);
anglesMatrix[0, 0] = anglesFiltered[0];
anglesMatrix[1, 0] = anglesFiltered[1];
anglesMatrix[2, 0] = anglesFiltered[2];
MatrixLibrary.Matrix q = MyQuaternion.getQuaternionFromAngles(anglesMatrix);
worldMatrix = Matrix.CreateFromQuaternion(AuxFrame * new Quaternion((float)q[1, 0], -(float)q[2, 0], (float)q[3, 0], -(float)q[0, 0]));
//worldMatrix = Matrix.CreateFromYawPitchRoll((float)(anglesFiltered[1] * Math.PI / 180), -(float)(anglesFiltered[0] * Math.PI / 180), -(float)(anglesFiltered[2] * Math.PI / 180));
}
else
{
float q1 = (float)filter.getFilteredQuaternions()[1, 0];
float q2 = (float)filter.getFilteredQuaternions()[2, 0];
float q3 = (float)filter.getFilteredQuaternions()[3, 0];
float q0 = (float)filter.getFilteredQuaternions()[0, 0];
if (trend)
{
plotForm.AddDataToGraph(q0, q1, q2, q3);
}
worldMatrix = Matrix.CreateFromQuaternion(AuxFrame * new Quaternion(q1, q2, q3, q0));
}
cubeEffect.World = worldMatrix;
foreach (EffectPass pass in cubeEffect.CurrentTechnique.Passes)
{
pass.Begin();
cubeEffect.Texture = cube.shapeTexture;
cube.RenderShape(GraphicsDevice);
pass.End();
}
cubeEffect.End();
base.Draw(gameTime);
}
protected MatrixLibrary.Matrix magneticCompensation(MyQuaternion q, double m_x, double m_y, double m_z)
{
MatrixLibrary.Matrix h = new MatrixLibrary.Matrix(4, 1);
MatrixLibrary.Matrix temp;
//compute the direction of the magnetic field
MatrixLibrary.Matrix quaternion = q.getQuaternionAsVector();
MatrixLibrary.Matrix quaternion_conjugate = q.getConjugate();
//magnetic field compensation
temp = MyQuaternion.quaternionProduct(quaternion, new MyQuaternion(0.0, m_x, m_y, m_z).getQuaternionAsVector());
h = MyQuaternion.quaternionProduct(temp, quaternion_conjugate);
double bx = Math.Sqrt((h[1, 0] * h[1, 0] + h[2, 0] * h[2, 0]));
double bz = h[3, 0];
double norm = Math.Sqrt(bx * bx + bz * bz);
bx /= norm;
bz /= norm;
MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(3, 1);
result[0, 0] = bx;
result[1, 0] = 0;
result[2, 0] = bz;
return(result);
}
public static MatrixLibrary.Matrix quaternionProduct(MyQuaternion q_a, MyQuaternion q_b)
{
MatrixLibrary.Matrix v1 = q_a.getQuaternionAsVector();
MatrixLibrary.Matrix v2 = q_b.getQuaternionAsVector();
return(quaternionProduct(v1, v2));
}
public override void filterStep(double w_x, double w_y, double w_z, double a_x, double a_y, double a_z, double m_x, double m_y, double m_z)
{
double norm;
Matrix temp;//=new Matrix(4,4);
Matrix dq = new Matrix(4, 1);
double mu;
double k;
// normalise the accelerometer measurement
norm = Math.Sqrt(a_x * a_x + a_y * a_y + a_z * a_z);
a_x /= norm;
a_y /= norm;
a_z /= norm;
// normalise the magnetometer measurement
norm = Math.Sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
m_x /= norm;
m_y /= norm;
m_z /= norm;
if (AccObservX.Count < 10)
{
AccObservX.Add(a_x);
AccObservY.Add(a_y);
AccObservZ.Add(a_z);
}
else
{
AccObservX.RemoveAt(0);
AccObservY.RemoveAt(0);
AccObservZ.RemoveAt(0);
AccObservX.Add(a_x);
AccObservY.Add(a_y);
AccObservZ.Add(a_z);
}
if (MagnObservX.Count < 10)
{
MagnObservX.Add(m_x);
MagnObservY.Add(m_y);
MagnObservZ.Add(m_z);
}
else
{
MagnObservX.RemoveAt(0);
MagnObservY.RemoveAt(0);
MagnObservZ.RemoveAt(0);
MagnObservX.Add(m_x);
MagnObservY.Add(m_y);
MagnObservZ.Add(m_z);
//Filter stabilization
accFilter.Applyfilter(AccObservX, out AccFiltX);
accFilter.Applyfilter(AccObservY, out AccFiltY);
accFilter.Applyfilter(AccObservZ, out AccFiltZ);
magnFilter.Applyfilter(MagnObservX, out MagnFiltX);
magnFilter.Applyfilter(MagnObservY, out MagnFiltY);
magnFilter.Applyfilter(MagnObservZ, out MagnFiltZ);
}
if (countdata > 10)
{
a_x = AccFiltX.Last();
a_y = AccFiltY.Last();
a_z = AccFiltZ.Last();
m_x = MagnFiltX.Last();
m_y = MagnFiltY.Last();
m_z = MagnFiltZ.Last();
// normalise the accelerometer measurement
norm = Math.Sqrt(a_x * a_x + a_y * a_y + a_z * a_z);
a_x /= norm;
a_y /= norm;
a_z /= norm;
// normalise the magnetometer measurement
norm = Math.Sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
m_x /= norm;
m_y /= norm;
m_z /= norm;
}
countdata++;
w_x = w_x - gyroOffRoll;
w_y = -w_y + gyroOffPitch;
w_z = w_z - gyroOffYaw;
w_x = w_x * Math.PI / 180;
w_y = w_y * Math.PI / 180;
w_z = w_z * Math.PI / 180;
if (this.obsMethod == 0)
{
dq = 0.5 * (MyQuaternion.quaternionProduct(state_observed, new MyQuaternion(0.0, w_x, w_y, w_z).getQuaternionAsVector()));
norm = Math.Sqrt(dq[0, 0] * dq[0, 0] + dq[1, 0] * dq[1, 0] + dq[2, 0] * dq[2, 0] + dq[3, 0] * dq[3, 0]);
mu = 10 * norm * dt;
state_observed = GradientDescent(a_x, a_y, a_z, m_x, m_y, m_z, mu, state_filtered);
}
else
{
state_observed = gaussNewtonMethod(a_x, a_y, a_z, m_x, m_y, m_z, state_filtered);
}
//KALMAN FILTERING
//F matrix computing
F[0, 0] = 1;
F[0, 1] = -dt / 2 * w_x;
F[0, 2] = -dt / 2 * w_y;
F[0, 3] = -dt / 2 * w_z;
F[1, 0] = dt / 2 * w_x;
F[1, 1] = 1;
F[1, 2] = dt / 2 * w_z;
F[1, 3] = -dt / 2 * w_y;
F[2, 0] = dt / 2 * w_y;
F[2, 1] = -dt / 2 * w_z;
F[2, 2] = 1;
F[2, 3] = dt / 2 * w_x;
F[3, 0] = dt / 2 * w_z;
F[3, 1] = dt / 2 * w_y;
F[3, 2] = -dt / 2 * w_x;
F[3, 3] = 1;
//state prediction
state_predicted = Matrix.Multiply(F, state_filtered);
//calculate the variance of the prediction
P_predicted = Matrix.Multiply(F, P_Update);
P_predicted = Matrix.Multiply(P_predicted, Matrix.Transpose(F)) + Q;
//compute the gain of the filter K
temp = Matrix.Add(Matrix.Multiply(Matrix.Multiply(H, P_predicted), Matrix.Transpose(H)), R);
temp = Matrix.Inverse(temp);
K = Matrix.Multiply(Matrix.Multiply(P_predicted, Matrix.Transpose(H)), temp);
//update the state of the system (this is the output of the filter)
temp = Matrix.Subtract(state_observed, Matrix.Multiply(H, state_predicted));
state_filtered = Matrix.Add(state_predicted, Matrix.Multiply(K, temp));
//compute the variance of the state filtered
temp = Matrix.Subtract((new Matrix(Matrix.Identity(4))), Matrix.Multiply(K, H));
P_Update = Matrix.Multiply(temp, P_predicted);
norm = Math.Sqrt(state_filtered[0, 0] * state_filtered[0, 0] + state_filtered[1, 0] * state_filtered[1, 0] + state_filtered[2, 0] * state_filtered[2, 0] + state_filtered[3, 0] * state_filtered[3, 0]);
state_filtered = Matrix.ScalarDivide(norm, state_filtered);
q_filt1 = state_filtered[0, 0];
q_filt2 = state_filtered[1, 0];
q_filt3 = state_filtered[2, 0];
q_filt4 = state_filtered[3, 0];
}
protected MatrixLibrary.Matrix GradientDescent(double a_x, double a_y, double a_z, double m_x, double m_y, double m_z, double mu, MatrixLibrary.Matrix qObserv)
{
int i = 0;
double q1, q2, q3, q4;
MatrixLibrary.Matrix f_obb = new MatrixLibrary.Matrix(6, 1);
MatrixLibrary.Matrix Jacobian = new MatrixLibrary.Matrix(6, 4);
MatrixLibrary.Matrix Df = new MatrixLibrary.Matrix(4, 1);
double norm;
double bx, bz, by;
MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(4, 1);
q1 = qObserv[0, 0];
q2 = qObserv[1, 0];
q3 = qObserv[2, 0];
q4 = qObserv[3, 0];
//Magnetometer Calibration values
m_x = m_x * magnSFX + magnOffX;
m_y = m_y * magnSFY + magnOffY;
m_z = m_z * magnSFZ + magnOffZ;
norm = Math.Sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
m_x /= norm;
m_y /= norm;
m_z /= norm;
while (i < 10)
{
//compute the direction of the magnetic field
MatrixLibrary.Matrix quaternion = new MyQuaternion(q1, q2, q3, q4).getQuaternionAsVector();
MatrixLibrary.Matrix bRif = magneticCompensation(new MyQuaternion(q1, q2, q3, q4), m_x, m_y, m_z);
bx = bRif[0, 0];
by = bRif[1, 0];
bz = bRif[2, 0];
//compute the objective functions
f_obb[0, 0] = 2 * (q2 * q4 - q1 * q3) - a_x;
f_obb[1, 0] = 2 * (q1 * q2 + q3 * q4) - a_y;
f_obb[2, 0] = 2 * (0.5 - q2 * q2 - q3 * q3) - a_z;
f_obb[3, 0] = 2 * bx * (0.5 - q3 * q3 - q4 * q4) + 2 * by * (q1 * q4 + q2 * q3) + 2 * bz * (q2 * q4 - q1 * q3) - m_x;
f_obb[4, 0] = 2 * bx * (q2 * q3 - q1 * q4) + 2 * by * (0.5 - q2 * q2 - q4 * q4) + 2 * bz * (q1 * q2 + q3 * q4) - m_y;
f_obb[5, 0] = 2 * bx * (q1 * q3 + q2 * q4) + 2 * by * (q3 * q4 - q1 * q2) + 2 * bz * (0.5 - q2 * q2 - q3 * q3) - m_z;
//compute the jacobian
Jacobian[0, 0] = -2 * q3;
Jacobian[0, 1] = 2 * q4;
Jacobian[0, 2] = -2 * q1;
Jacobian[0, 3] = 2 * q2;
Jacobian[1, 0] = 2 * q2;
Jacobian[1, 1] = 2 * q1;
Jacobian[1, 2] = 2 * q4;
Jacobian[1, 3] = 2 * q3;
Jacobian[2, 0] = 0;
Jacobian[2, 1] = -4 * q2;
Jacobian[2, 2] = -4 * q3;
Jacobian[2, 3] = 0;
Jacobian[3, 0] = 2 * by * q4 - 2 * bz * q3;
Jacobian[3, 1] = 2 * by * q3 + 2 * bz * q4;
Jacobian[3, 2] = -4 * bx * q3 + 2 * by * q2 - 2 * bz * q1;
Jacobian[3, 3] = -4 * bx * q4 + 2 * by * q1 + 2 * bz * q2;
Jacobian[4, 0] = -2 * bx * q4 + 2 * bz * q2;
Jacobian[4, 1] = 2 * bx * q3 - 4 * by * q2 + 2 * bz * q1;
Jacobian[4, 2] = 2 * bx * q2 + 2 * bz * q4;
Jacobian[4, 3] = -2 * bx * q1 - 4 * by * q4 + 2 * bz * q3;
Jacobian[5, 0] = 2 * bx * q3 - 2 * by * q2;
Jacobian[5, 1] = 2 * bx * q4 - 2 * by * q1 - 4 * bz * q2;
Jacobian[5, 2] = 2 * bx * q1 + 2 * by * q4 - 4 * bz * q3;
Jacobian[5, 3] = 2 * bx * q2 + 2 * by * q3;
Df = MatrixLibrary.Matrix.Multiply(MatrixLibrary.Matrix.Transpose(Jacobian), f_obb);
norm = Math.Sqrt(Df[0, 0] * Df[0, 0] + Df[1, 0] * Df[1, 0] + Df[2, 0] * Df[2, 0] + Df[3, 0] * Df[3, 0]);
Df = MatrixLibrary.Matrix.ScalarDivide(norm, Df);
result = quaternion - mu * Df;
q1 = result[0, 0];
q2 = result[1, 0];
q3 = result[2, 0];
q4 = result[3, 0];
norm = Math.Sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
result = MatrixLibrary.Matrix.ScalarDivide(norm, result);
q1 = result[0, 0];
q2 = result[1, 0];
q3 = result[2, 0];
q4 = result[3, 0];
i = i + 1;
}
return(result);
}
protected MatrixLibrary.Matrix gaussNewtonMethod(double a_x, double a_y, double a_z, double m_x, double m_y, double m_z, MatrixLibrary.Matrix qObserv)
{
double norm;
MatrixLibrary.Matrix n = new MatrixLibrary.Matrix(4, 1);
MatrixLibrary.Matrix bRif = new MatrixLibrary.Matrix(3, 1);
MatrixLibrary.Matrix jacobian = new MatrixLibrary.Matrix(6, 4);
MatrixLibrary.Matrix R = new MatrixLibrary.Matrix(6, 6);
MatrixLibrary.Matrix y_e = new MatrixLibrary.Matrix(6, 1);
MatrixLibrary.Matrix y_b = new MatrixLibrary.Matrix(6, 1);
double a = qObserv[1, 0];
double b = qObserv[2, 0];
double c = qObserv[3, 0];
double d = qObserv[0, 0];
int i = 0;
MatrixLibrary.Matrix n_k = new MatrixLibrary.Matrix(4, 1);
n_k[0, 0] = a;
n_k[1, 0] = b;
n_k[2, 0] = c;
n_k[3, 0] = d;
//Magnetometer Calibration values
m_x = m_x * magnSFX + magnOffX;
m_y = m_y * magnSFY + magnOffY;
m_z = m_z * magnSFZ + magnOffZ;
norm = Math.Sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
m_x /= norm;
m_y /= norm;
m_z /= norm;
while (i < 3)
{
MyQuaternion q = new MyQuaternion(d, a, b, c);
bRif = magneticCompensation(q, m_x, m_y, m_z);
double bx = bRif[0, 0];
double by = bRif[1, 0];
double bz = bRif[2, 0];
//Jacobian Computation
double j11 = (2 * a * a_x + 2 * b * a_y + 2 * c * a_z);
double j12 = (-2 * b * a_x + 2 * a * a_y + 2 * d * a_z);
double j13 = (-2 * c * a_x - 2 * d * a_y + 2 * a * a_z);
double j14 = (2 * d * a_x - 2 * c * a_y + 2 * b * a_z);
double j21 = (2 * b * a_x - 2 * a * a_y - 2 * d * a_z);
double j22 = (2 * a * a_x + 2 * b * a_y + 2 * c * a_z);
double j23 = (2 * d * a_x - 2 * c * a_y + 2 * b * a_z);
double j24 = (2 * c * a_x + 2 * d * a_y - 2 * a * a_z);
double j31 = (2 * c * a_x + 2 * d * a_y - 2 * a * a_z);
double j32 = (-2 * d * a_x + 2 * c * a_y - 2 * b * a_z);
double j33 = (2 * a * a_x + 2 * b * a_y + 2 * c * a_z);
double j34 = (-2 * b * a_x + 2 * a * a_y + 2 * d * a_z);
double j41 = (2 * a * m_x + 2 * b * m_y + 2 * c * m_z);
double j42 = (-2 * b * m_x + 2 * a * m_y + 2 * m_z * d);
double j43 = (-2 * c * m_x - 2 * d * m_y + 2 * a * m_z);
double j44 = (2 * d * m_x - 2 * c * m_y + 2 * b * m_z);
double j51 = (2 * b * m_x - 2 * a * m_y - 2 * d * m_z);
double j52 = (2 * a * m_x + 2 * b * m_y + 2 * c * m_z);
double j53 = (2 * d * m_x - 2 * c * m_y + 2 * b * m_z);
double j54 = (2 * c * m_x + 2 * d * m_y - 2 * a * m_z);
double j61 = (2 * c * m_x + 2 * d * m_y - 2 * a * m_z);
double j62 = (-2 * d * m_x + 2 * c * m_y - 2 * b * m_z);
double j63 = (2 * a * m_x + 2 * b * m_y + 2 * c * m_z);
double j64 = (-2 * b * m_x + 2 * a * m_y + 2 * d * m_z);
jacobian[0, 0] = j11;
jacobian[0, 1] = j12;
jacobian[0, 2] = j13;
jacobian[0, 3] = j14;
jacobian[1, 0] = j21;
jacobian[1, 1] = j22;
jacobian[1, 2] = j23;
jacobian[1, 3] = j24;
jacobian[2, 0] = j31;
jacobian[2, 1] = j32;
jacobian[2, 2] = j33;
jacobian[2, 3] = j34;
jacobian[3, 0] = j41;
jacobian[3, 1] = j42;
jacobian[3, 2] = j43;
jacobian[3, 3] = j44;
jacobian[4, 0] = j51;
jacobian[4, 1] = j52;
jacobian[4, 2] = j53;
jacobian[4, 3] = j54;
jacobian[5, 0] = j61;
jacobian[5, 1] = j62;
jacobian[5, 2] = j63;
jacobian[5, 3] = j64;
jacobian = -1 * jacobian;
//End Jacobian Computation
//DCM Rotation Matrix
R[0, 0] = d * d + a * a - b * b - c * c;
R[0, 1] = 2 * (a * b - c * d);
R[0, 2] = 2 * (a * c + b * d);
R[1, 0] = 2 * (a * b + c * d);
R[1, 1] = d * d + b * b - a * a - c * c;
R[1, 2] = 2 * (b * c - a * d);
R[2, 0] = 2 * (a * c - b * d);
R[2, 1] = 2 * (b * c + a * d);
R[2, 2] = d * d + c * c - b * b - a * a;
R[3, 3] = d * d + a * a - b * b - c * c;
R[3, 4] = 2 * (a * b - c * d);
R[3, 5] = 2 * (a * c + b * d);
R[4, 3] = 2 * (a * b + c * d);
R[4, 4] = d * d + b * b - a * a - c * c;
R[4, 5] = 2 * (b * c - a * d);
R[5, 3] = 2 * (a * c - b * d);
R[5, 4] = 2 * (b * c + a * d);
R[5, 5] = d * d + c * c - b * b - a * a;
R[3, 0] = 0;
R[3, 1] = 0;
R[3, 2] = 0;
R[4, 0] = 0;
R[4, 1] = 0;
R[4, 2] = 0;
R[5, 0] = 0;
R[5, 1] = 0;
R[5, 2] = 0;
R[0, 3] = 0;
R[0, 4] = 0;
R[0, 5] = 0;
R[1, 3] = 0;
R[1, 4] = 0;
R[1, 5] = 0;
R[2, 3] = 0;
R[2, 4] = 0;
R[2, 5] = 0;
//End DCM
//Reference Vector
y_e[0, 0] = 0;
y_e[1, 0] = 0;
y_e[2, 0] = 1;
y_e[3, 0] = bx;
y_e[4, 0] = by;
y_e[5, 0] = bz;
//Body frame Vector
y_b[0, 0] = a_x;
y_b[1, 0] = a_y;
y_b[2, 0] = a_z;
y_b[3, 0] = m_x;
y_b[4, 0] = m_y;
y_b[5, 0] = m_z;
//Gauss Newton Step
n = n_k - MatrixLibrary.Matrix.Inverse((MatrixLibrary.Matrix.Transpose(jacobian) * jacobian)) * MatrixLibrary.Matrix.Transpose(jacobian) * (y_e - R * y_b);
double normGauss = Math.Sqrt(n[0, 0] * n[0, 0] + n[1, 0] * n[1, 0] + n[2, 0] * n[2, 0] + n[3, 0] * n[3, 0]);
n /= normGauss;
//Console.Out.WriteLine(n + " "+ norm);
a = n[0, 0];
b = n[1, 0];
c = n[2, 0];
d = n[3, 0];
n_k[0, 0] = a;
n_k[1, 0] = b;
n_k[2, 0] = c;
n_k[3, 0] = d;
i++;
}
return(new MyQuaternion(d, a, b, c).getQuaternionAsVector());
}
public override void filterStep(double w_x, double w_y, double w_z, double a_x, double a_y, double a_z, double m_x, double m_y, double m_z)
{
// normalise the accelerometer measurement
double norm = Math.Sqrt(a_x * a_x + a_y * a_y + a_z * a_z);
a_x /= norm;
a_y /= norm;
a_z /= norm;
// normalise the magnetometer measurement
norm = Math.Sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
m_x /= norm;
m_y /= norm;
m_z /= norm;
if (AccObservX.Count < 10)
{
AccObservX.Add(a_x);
AccObservY.Add(a_y);
AccObservZ.Add(a_z);
}
else
{
AccObservX.RemoveAt(0);
AccObservY.RemoveAt(0);
AccObservZ.RemoveAt(0);
AccObservX.Add(a_x);
AccObservY.Add(a_y);
AccObservZ.Add(a_z);
}
if (MagnObservX.Count < 10)
{
MagnObservX.Add(m_x);
MagnObservY.Add(m_y);
MagnObservZ.Add(m_z);
}
else
{
MagnObservX.RemoveAt(0);
MagnObservY.RemoveAt(0);
MagnObservZ.RemoveAt(0);
MagnObservX.Add(m_x);
MagnObservY.Add(m_y);
MagnObservZ.Add(m_z);
//Filter stabilization
accFilter.Applyfilter(AccObservX, out AccFiltX);
accFilter.Applyfilter(AccObservY, out AccFiltY);
accFilter.Applyfilter(AccObservZ, out AccFiltZ);
magnFilter.Applyfilter(MagnObservX, out MagnFiltX);
magnFilter.Applyfilter(MagnObservY, out MagnFiltY);
magnFilter.Applyfilter(MagnObservZ, out MagnFiltZ);
}
if (step < 10)
{
}
else
{
accFilter.Applyfilter(AccObservX, out AccFiltX);
accFilter.Applyfilter(AccObservY, out AccFiltY);
accFilter.Applyfilter(AccObservZ, out AccFiltZ);
magnFilter.Applyfilter(MagnObservX, out MagnFiltX);
magnFilter.Applyfilter(MagnObservY, out MagnFiltY);
magnFilter.Applyfilter(MagnObservZ, out MagnFiltZ);
a_x = AccFiltX.Last();
a_y = AccFiltY.Last();
a_z = AccFiltZ.Last();
m_x = MagnFiltX.Last();
m_y = MagnFiltY.Last();
m_z = MagnFiltZ.Last();
//Magnetometer Calibration values
//m_x =m_x*1.2+ 0.3;
//m_y = m_y*1.1+0.04;
//m_z += -0.08;
double normA = Math.Sqrt(a_x * a_x + a_y * a_y + a_z * a_z);
double normM = Math.Sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
a_x /= normA;
a_y /= normA;
a_z /= normA;
m_x /= normM;
m_y /= normM;
m_z /= normM;
w_x = (w_x - gyroOffRoll) * Math.PI / 180;
w_y = (-w_y + gyroOffPitch) * Math.PI / 180;
w_z = (w_z - gyroOffYaw) * Math.PI / 180;
MatrixLibrary.Matrix dq = 0.5 * (MyQuaternion.quaternionProduct(qFilt, new MyQuaternion(0, w_x, w_y, w_z).getQuaternionAsVector()));
qGyroFilt = qFilt + dq * dt;
norm = Math.Sqrt(qGyroFilt[0, 0] * qGyroFilt[0, 0] + qGyroFilt[1, 0] * qGyroFilt[1, 0] + qGyroFilt[2, 0] * qGyroFilt[2, 0] + qGyroFilt[3, 0] * qGyroFilt[3, 0]);
qGyroFilt /= norm;
double dqNorm = Math.Sqrt(dq[0, 0] * dq[0, 0] + dq[1, 0] * dq[1, 0] + dq[2, 0] * dq[2, 0] + dq[3, 0] * dq[3, 0]);
double mu = 10 * dqNorm * dt;
if (this.obsMethod == 0)
{
qObserv = GradientDescent(a_x, a_y, a_z, m_x, m_y, m_z, mu, qFilt);
}
else
{
qObserv = gaussNewtonMethod(a_x, a_y, a_z, m_x, m_y, m_z, qFilt);
}
qFilt = qGyroFilt * k + qObserv * (1 - k);
norm = Math.Sqrt(qFilt[0, 0] * qFilt[0, 0] + qFilt[1, 0] * qFilt[1, 0] + qFilt[2, 0] * qFilt[2, 0] + qFilt[3, 0] * qFilt[3, 0]);
qFilt /= norm;
}
step++;
}