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)
{
a_x = a_x / 1000;
a_y = a_y / 1000;
a_z = a_z / 1000;
// 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)
{
anglesEuler = computeAnglesFromAccAndMagn(new double[] { a_x, a_y, a_z }, new double[] { m_x, m_y, m_z });
anglesFiltered[0] = anglesEuler[0];
anglesFiltered[1] = anglesEuler[1];
anglesFiltered[2] = anglesEuler[2];
anglesGyro[0] = anglesEuler[0];
anglesGyro[1] = anglesEuler[1];
anglesGyro[2] = anglesEuler[2];
anglesGyroFiltered[0] = anglesEuler[0];
anglesGyroFiltered[1] = anglesEuler[1];
anglesGyroFiltered[2] = anglesEuler[2];
}
else
{
//Filtering
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();
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;
w_y = w_y - gyroOffPitch;
w_z = -w_z + gyroOffYaw;
//w_x = (w_x + w_x_old) / 2;
//w_y = (w_y + w_y_old) / 2;
//w_z = (w_z + w_z_old) / 2;
anglesGyro[0] = anglesGyro[0] + w_x * dt;
anglesGyro[1] = anglesGyro[1] + w_y * dt;
anglesGyro[2] = anglesGyro[2] + w_z * dt;
double[] dEulerGyroFiltered = computeEulerFromGyro(new double[] { w_x, w_y, w_z }, anglesFiltered);
dEulerGyroFiltered[0] = dEulerGyroFiltered[0] * dt;
dEulerGyroFiltered[1] = dEulerGyroFiltered[1] * dt;
dEulerGyroFiltered[2] = dEulerGyroFiltered[2] * dt;
anglesGyroFiltered[0] = anglesFiltered[0] + dEulerGyroFiltered[0];
anglesGyroFiltered[1] = anglesFiltered[1] + dEulerGyroFiltered[1];
anglesGyroFiltered[2] = anglesFiltered[2] + dEulerGyroFiltered[2];
anglesEuler = computeAnglesFromAccAndMagn(new double[] { a_x, a_y, a_z }, new double[] { m_x, m_y, m_z });
for (int j = 0; j < 3; j++)
{
if ((180 - Math.Abs(anglesEuler[j])) < 15 && (180 - Math.Abs(anglesGyroFiltered[j])) < 15)
{
if (Math.Sign(anglesEuler[j]) != Math.Sign(anglesGyroFiltered[j]))
{
anglesGyroFiltered[j] = Math.Sign(anglesEuler[j]) * (360 - Math.Abs(anglesGyroFiltered[j]));
}
}
}
anglesFiltered[0] = k * anglesGyroFiltered[0] + (1 - k) * anglesEuler[0];
anglesFiltered[1] = k * anglesGyroFiltered[1] + (1 - k) * anglesEuler[1];
anglesFiltered[2] = k * anglesGyroFiltered[2] + (1 - k) * anglesEuler[2];
anglesFiltered = limitRotations(anglesFiltered);
anglesGyroFiltered = limitRotations(anglesGyroFiltered);
}
step++;
}
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++;
}
