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 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];
}
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++;
}
