public ComplementaryFilter()
{
k = 0.98;
anglesGyro = new double[3];
anglesEuler = new double[3];
anglesFiltered = new double[3];
anglesGyroFiltered = new double[3];
aAcc = new List <double>();
bAcc = new List <double>();
aMagn = new List <double>();
bMagn = new List <double>();
aAcc.Add(1);
aAcc.Add(-2.9529);
aAcc.Add(2.9069);
aAcc.Add(-0.954);
bAcc.Add(0.000001597);
bAcc.Add(0.000004792);
bAcc.Add(0.000004792);
bAcc.Add(0.000001597);
accFilter = new IIRFilter(aAcc, bAcc);
aMagn.Add(1);
aMagn.Add(-1.73);
aMagn.Add(0.76);
bMagn.Add(0.0078);
bMagn.Add(0.0156);
bMagn.Add(0.0078);
magnFilter = new IIRFilter(aMagn, bMagn);
AccObservX = new List <double>();
AccObservY = new List <double>();
AccObservZ = new List <double>();
AccFiltX = new List <double>();
AccFiltY = new List <double>();
AccFiltZ = new List <double>();
MagnObservX = new List <double>();
MagnObservY = new List <double>();
MagnObservZ = new List <double>();
MagnFiltX = new List <double>();
MagnFiltY = new List <double>();
MagnFiltZ = new List <double>();
}
public QuaternionCF()
{
k = 0.98;
aAcc = new List <double>();
bAcc = new List <double>();
aMagn = new List <double>();
bMagn = new List <double>();
aAcc.Add(1);
aAcc.Add(-2.9529);
aAcc.Add(2.9069);
aAcc.Add(-0.954);
bAcc.Add(0.000001597);
bAcc.Add(0.000004792);
bAcc.Add(0.000004792);
bAcc.Add(0.000001597);
accFilter = new IIRFilter(aAcc, bAcc);
aMagn.Add(1);
aMagn.Add(-1.73);
aMagn.Add(0.76);
bMagn.Add(0.0078);
bMagn.Add(0.0156);
bMagn.Add(0.0078);
magnFilter = new IIRFilter(aMagn, bMagn);
AccObservX = new List <double>();
AccObservY = new List <double>();
AccObservZ = new List <double>();
AccFiltX = new List <double>();
AccFiltY = new List <double>();
AccFiltZ = new List <double>();
MagnObservX = new List <double>();
MagnObservY = new List <double>();
MagnObservZ = new List <double>();
MagnFiltX = new List <double>();
MagnFiltY = new List <double>();
MagnFiltZ = new List <double>();
qFilt = new MatrixLibrary.Matrix(4, 1);
qGyroFilt = new MatrixLibrary.Matrix(4, 1);
qObserv = new MatrixLibrary.Matrix(4, 1);
qFilt[0, 0] = 1;
qFilt[1, 0] = 0;
qFilt[2, 0] = 0;
qFilt[3, 0] = 0;
qGyroFilt[0, 0] = 1;
qGyroFilt[1, 0] = 0;
qGyroFilt[2, 0] = 0;
qGyroFilt[3, 0] = 0;
qObserv[0, 0] = 1;
qObserv[1, 0] = 0;
qObserv[2, 0] = 0;
qObserv[3, 0] = 0;
}
public KalmanFilter()
{
//Compute matrix Q (costant if the time interval is costant)
Q[0, 0] = sigmaRoll + sigmaPitch + sigmaYaw;
Q[0, 1] = -sigmaRoll + sigmaPitch - sigmaYaw;
Q[0, 2] = -sigmaRoll - sigmaPitch + sigmaYaw;
Q[0, 3] = sigmaRoll - sigmaPitch - sigmaYaw;
Q[1, 0] = -sigmaRoll + sigmaPitch - sigmaYaw;
Q[1, 1] = sigmaRoll + sigmaPitch + sigmaYaw;
Q[1, 2] = sigmaRoll - sigmaPitch - sigmaYaw;
Q[1, 3] = -sigmaRoll - sigmaPitch + sigmaYaw;
Q[2, 0] = -sigmaRoll - sigmaPitch + sigmaYaw;
Q[2, 1] = sigmaRoll - sigmaPitch - sigmaYaw;
Q[2, 2] = sigmaRoll + sigmaPitch + sigmaYaw;
Q[2, 3] = -sigmaRoll + sigmaPitch - sigmaYaw;
Q[3, 0] = sigmaRoll - sigmaPitch - sigmaYaw;
Q[3, 1] = -sigmaRoll - sigmaPitch + sigmaYaw;
Q[3, 2] = -sigmaRoll + sigmaPitch - sigmaYaw;
Q[3, 3] = sigmaRoll + sigmaPitch + sigmaYaw;
R = new Matrix(4, 4);
R[0, 0] = 0.5;
R[0, 1] = 0;
R[0, 2] = 0;
R[0, 3] = 0;
R[1, 0] = 0;
R[1, 1] = 0.5;
R[1, 2] = 0;
R[1, 3] = 0;
R[2, 0] = 0;
R[2, 1] = 0;
R[2, 2] = 0.5;
R[2, 3] = 0;
R[3, 0] = 0;
R[3, 1] = 0;
R[3, 2] = 0;
R[3, 3] = 0.5;
//Initial conditions
state_observed = new MyQuaternion(0.5, 0.5, 0.5, 0.5).getQuaternionAsVector();
state_filtered = new MyQuaternion(0.5, 0.5, 0.5, 0.5).getQuaternionAsVector();
state_predicted = new MyQuaternion(0.5, 0.5, 0.5, 0.5).getQuaternionAsVector();
P_Update = Matrix.ScalarMultiply(0.1, new Matrix(Matrix.Identity(4)));
aAcc = new List <double>();
bAcc = new List <double>();
aMagn = new List <double>();
bMagn = new List <double>();
aAcc.Add(1);
aAcc.Add(-2.9529);
aAcc.Add(2.9069);
aAcc.Add(-0.954);
bAcc.Add(0.000001597);
bAcc.Add(0.000004792);
bAcc.Add(0.000004792);
bAcc.Add(0.000001597);
accFilter = new IIRFilter(aAcc, bAcc);
aMagn.Add(1);
aMagn.Add(-1.73);
aMagn.Add(0.76);
bMagn.Add(0.0078);
bMagn.Add(0.0156);
bMagn.Add(0.0078);
magnFilter = new IIRFilter(aMagn, bMagn);
AccObservX = new List <double>();
AccObservY = new List <double>();
AccObservZ = new List <double>();
AccFiltX = new List <double>();
AccFiltY = new List <double>();
AccFiltZ = new List <double>();
MagnObservX = new List <double>();
MagnObservY = new List <double>();
MagnObservZ = new List <double>();
MagnFiltX = new List <double>();
MagnFiltY = new List <double>();
MagnFiltZ = new List <double>();
weOld = new Matrix(4, 1);
weOld = Matrix.ScalarMultiply(0, weOld);
}
public KalmanFilter()
{
//Compute matrix Q (costant if the time interval is costant)
Q[0, 0] = sigmaRoll + sigmaPitch + sigmaYaw;
Q[0, 1] = -sigmaRoll + sigmaPitch - sigmaYaw;
Q[0, 2] = -sigmaRoll - sigmaPitch + sigmaYaw;
Q[0, 3] = sigmaRoll - sigmaPitch - sigmaYaw;
Q[1, 0] = -sigmaRoll + sigmaPitch - sigmaYaw;
Q[1, 1] = sigmaRoll + sigmaPitch + sigmaYaw;
Q[1, 2] = sigmaRoll - sigmaPitch - sigmaYaw;
Q[1, 3] = -sigmaRoll - sigmaPitch + sigmaYaw;
Q[2, 0] = -sigmaRoll - sigmaPitch + sigmaYaw;
Q[2, 1] = sigmaRoll - sigmaPitch - sigmaYaw;
Q[2, 2] = sigmaRoll + sigmaPitch + sigmaYaw;
Q[2, 3] = -sigmaRoll + sigmaPitch - sigmaYaw;
Q[3, 0] = sigmaRoll - sigmaPitch - sigmaYaw;
Q[3, 1] = -sigmaRoll - sigmaPitch + sigmaYaw;
Q[3, 2] = -sigmaRoll + sigmaPitch - sigmaYaw;
Q[3, 3] = sigmaRoll + sigmaPitch + sigmaYaw;
R = new Matrix(4, 4);
R[0, 0] = 0.5;
R[0, 1] = 0;
R[0, 2] = 0;
R[0, 3] = 0;
R[1, 0] = 0;
R[1, 1] = 0.5;
R[1, 2] = 0;
R[1, 3] = 0;
R[2, 0] = 0;
R[2, 1] = 0;
R[2, 2] = 0.5;
R[2, 3] = 0;
R[3, 0] = 0;
R[3, 1] = 0;
R[3, 2] = 0;
R[3, 3] = 0.5;
//Initial conditions
state_observed = new MyQuaternion(0.5, 0.5, 0.5, 0.5).getQuaternionAsVector();
state_filtered = new MyQuaternion(0.5, 0.5, 0.5, 0.5).getQuaternionAsVector();
state_predicted = new MyQuaternion(0.5, 0.5, 0.5, 0.5).getQuaternionAsVector();
P_Update = Matrix.ScalarMultiply(0.1, new Matrix(Matrix.Identity(4)));
aAcc = new List<double>();
bAcc = new List<double>();
aMagn = new List<double>();
bMagn = new List<double>();
aAcc.Add(1);
aAcc.Add(-2.9529);
aAcc.Add(2.9069);
aAcc.Add(-0.954);
bAcc.Add(0.000001597);
bAcc.Add(0.000004792);
bAcc.Add(0.000004792);
bAcc.Add(0.000001597);
accFilter = new IIRFilter(aAcc, bAcc);
aMagn.Add(1);
aMagn.Add(-1.73);
aMagn.Add(0.76);
bMagn.Add(0.0078);
bMagn.Add(0.0156);
bMagn.Add(0.0078);
magnFilter = new IIRFilter(aMagn, bMagn);
AccObservX = new List<double>();
AccObservY = new List<double>();
AccObservZ = new List<double>();
AccFiltX = new List<double>();
AccFiltY = new List<double>();
AccFiltZ = new List<double>();
MagnObservX = new List<double>();
MagnObservY = new List<double>();
MagnObservZ = new List<double>();
MagnFiltX = new List<double>();
MagnFiltY = new List<double>();
MagnFiltZ = new List<double>();
weOld = new Matrix(4, 1);
weOld = Matrix.ScalarMultiply(0, weOld);
}
public QuaternionCF()
{
k = 0.98;
aAcc = new List<double>();
bAcc = new List<double>();
aMagn = new List<double>();
bMagn = new List<double>();
aAcc.Add(1);
aAcc.Add(-2.9529);
aAcc.Add(2.9069);
aAcc.Add(-0.954);
bAcc.Add(0.000001597);
bAcc.Add(0.000004792);
bAcc.Add(0.000004792);
bAcc.Add(0.000001597);
accFilter = new IIRFilter(aAcc, bAcc);
aMagn.Add(1);
aMagn.Add(-1.73);
aMagn.Add(0.76);
bMagn.Add(0.0078);
bMagn.Add(0.0156);
bMagn.Add(0.0078);
magnFilter = new IIRFilter(aMagn, bMagn);
AccObservX = new List<double>();
AccObservY = new List<double>();
AccObservZ = new List<double>();
AccFiltX = new List<double>();
AccFiltY = new List<double>();
AccFiltZ = new List<double>();
MagnObservX = new List<double>();
MagnObservY = new List<double>();
MagnObservZ = new List<double>();
MagnFiltX = new List<double>();
MagnFiltY = new List<double>();
MagnFiltZ = new List<double>();
qFilt = new MatrixLibrary.Matrix(4, 1);
qGyroFilt = new MatrixLibrary.Matrix(4, 1);
qObserv = new MatrixLibrary.Matrix(4, 1);
qFilt[0, 0] = 1;
qFilt[1, 0] = 0;
qFilt[2, 0] = 0;
qFilt[3, 0] = 0;
qGyroFilt[0, 0] = 1;
qGyroFilt[1, 0] = 0;
qGyroFilt[2, 0] = 0;
qGyroFilt[3, 0] = 0;
qObserv[0, 0] = 1;
qObserv[1, 0] = 0;
qObserv[2, 0] = 0;
qObserv[3, 0] = 0;
}