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