public ReadingsVector3f(ReadingsVector3f original)
{
var size = 3;
this.Values = new float[size];
for (int i = 0; i < size; ++i)
{
this.Values[i] = original.Values[i];
}
this.TimeStampI = original.TimeStampI;
}
protected ReadingsVector3f Get3fReadings(FileStream stream)
{
ReadingsVector3f res = new ReadingsVector3f();
try
{
if (stream.Length <= stream.Position)
{
res.IsEmpty = true;
}
else
{
byte[] buff = new byte[16];
stream.Read(buff, 0, 16);
res.TimeStampI = Convert.ToInt64(this.ReadingsBytesToUInt(buff.Take(4).ToArray()));
res.Values[0] = this.ReadingsBytesToFloat(buff.Skip(4).Take(4).ToArray());
res.Values[1] = this.ReadingsBytesToFloat(buff.Skip(8).Take(4).ToArray());
res.Values[2] = this.ReadingsBytesToFloat(buff.Skip(12).Take(4).ToArray());
}
}
catch
{
res.IsEmpty = true;
}
return res;
}
protected ReadingsVector3f Get3fReadings(BinaryReader stream)
{
ReadingsVector3f res = new ReadingsVector3f();
try
{
if (stream.BaseStream.Length <= stream.BaseStream.Position)
{
res.IsEmpty = true;
}
else
{
res.TimeStampI = Convert.ToInt64(this.ReadingsBytesToUInt(stream.ReadBytes(4)));
res.Values[0] = this.ReadingsBytesToFloat(stream.ReadBytes(4));
res.Values[1] = this.ReadingsBytesToFloat(stream.ReadBytes(4));
res.Values[2] = this.ReadingsBytesToFloat(stream.ReadBytes(4));
}
}
catch
{
res.IsEmpty = true;
}
return res;
}
protected ReadingsVector3f Get3fReadings(byte[] bufferPart)
{
ReadingsVector3f res = new ReadingsVector3f();
try
{
int ptr = 0;
if (bufferPart.Length < (sizeof(float) * 3 + sizeof(int)))
{
res.IsEmpty = true;
}
else
{
res.TimeStampI = Convert.ToInt64(this.ReadingsBytesToUInt(bufferPart.Take(sizeof(int)).ToArray()));
ptr += sizeof(int);
res.Values[0] = this.ReadingsBytesToFloat(bufferPart.Skip(ptr).Take(sizeof(float)).ToArray());
ptr += sizeof(float);
res.Values[1] = this.ReadingsBytesToFloat(bufferPart.Skip(ptr).Take(sizeof(float)).ToArray());
ptr += sizeof(float);
res.Values[2] = this.ReadingsBytesToFloat(bufferPart.Skip(ptr).Take(sizeof(float)).ToArray());
ptr += sizeof(float);
}
}
catch
{
res.IsEmpty = true;
}
return res;
}
public Matrix<double> GetGyroRotationMatrix(ReadingsVector3f gyroVect, double gyroSpeedMul)
{
Matrix<double> res = new Matrix<double>(3, 3);
if (this.OldGyroReadings == null)
{
res.SetIdentity();
}
else
{
long diffMS = gyroVect.TimeStampI - this.OldGyroReadings.TimeStampI;
double xr = -((double)gyroVect.Values[0] * diffMS) / 1000 * gyroSpeedMul;
double yr = ((double)gyroVect.Values[1] * diffMS) / 1000 * gyroSpeedMul;
double zr = -((double)gyroVect.Values[2] * diffMS) / 1000 * gyroSpeedMul;
double sinxr = Math.Sin(xr);
double cosxr = Math.Cos(xr);
double sinyr = Math.Sin(yr);
double cosyr = Math.Cos(yr);
double sinzr = Math.Sin(zr);
double coszr = Math.Cos(zr);
Matrix<double> xM = new Matrix<double>(new double[,]
{
{1, 0, 0 },
{0, cosxr, sinxr},
{0, -sinxr, cosxr}
});
Matrix<double> yM = new Matrix<double>(new double[,]
{
{cosyr, 0, sinyr},
{0, 1, 0 },
{-sinyr, 0, cosyr}
});
Matrix<double> zM = new Matrix<double>(new double[,]
{
{coszr, sinzr, 0},
{-sinzr, coszr, 0},
{0, 0, 1}
});
res = xM.Mul(yM).Mul(zM);
}
return res;
}
public double[][] GetAccMagnetOrientationMatrix(MEMSReadingsSet3f newReadings, bool useAccMagnet, bool useGyroscope, bool useLowpassFilter, bool useAdaptiveFiltering, double accMagnetFilterCoeff, double gyroFilterCoeff)
{
if (useAdaptiveFiltering)
{
this.CalcFilterCoeffs(newReadings, out gyroFilterCoeff, out accMagnetFilterCoeff, accMagnetFilterCoeff, gyroFilterCoeff);
}
double[][] res = new double[3][];
for (int i = 0; i < 3; ++i)
{
res[i] = new double[3];
}
MCvPoint3D64f rawAccPoint = new MCvPoint3D64f(newReadings.AccVector3f.Values[0], newReadings.AccVector3f.Values[1], newReadings.AccVector3f.Values[2]);
MCvPoint3D64f rawMagnetPoint = new MCvPoint3D64f(newReadings.MagnetVector3f.Values[0], newReadings.MagnetVector3f.Values[1], newReadings.MagnetVector3f.Values[2]);
double gyroSpeedMul = 1;
if (useAdaptiveFiltering)
{
if(useAccMagnet)
{
gyroSpeedMul = this.GyroSpeedMul;
}
}
var gyroDiffMatr = this.GetGyroRotationMatrix(newReadings.GyroVector3f, gyroSpeedMul);
MCvPoint3D64f accFilteredPoint;
MCvPoint3D64f magnetFilteredPoint;
if (useLowpassFilter && this.OldAMGFusedAcc != null && this.OldAMGFusedMagnet != null)
{
if (useAccMagnet)
{
//this.FilterAccMagnet(ref resAccPoint, ref resMagnetPoint, accMagnetFilterCoeff);
accFilteredPoint = new MCvPoint3D64f(
rawAccPoint.x * accMagnetFilterCoeff + OldAMGFusedAcc.Value.x * (1 - accMagnetFilterCoeff),
rawAccPoint.y * accMagnetFilterCoeff + OldAMGFusedAcc.Value.y * (1 - accMagnetFilterCoeff),
rawAccPoint.z * accMagnetFilterCoeff + OldAMGFusedAcc.Value.z * (1 - accMagnetFilterCoeff)
);
magnetFilteredPoint = new MCvPoint3D64f(
rawMagnetPoint.x * accMagnetFilterCoeff + OldAMGFusedMagnet.Value.x * (1 - accMagnetFilterCoeff),
rawMagnetPoint.y * accMagnetFilterCoeff + OldAMGFusedMagnet.Value.y * (1 - accMagnetFilterCoeff),
rawMagnetPoint.z * accMagnetFilterCoeff + OldAMGFusedMagnet.Value.z * (1 - accMagnetFilterCoeff)
);
}
else
{
accFilteredPoint = new MCvPoint3D64f(this.OldAMGFusedAcc.Value.x, this.OldAMGFusedAcc.Value.y, this.OldAMGFusedAcc.Value.z);
magnetFilteredPoint = new MCvPoint3D64f(this.OldAMGFusedMagnet.Value.x, this.OldAMGFusedMagnet.Value.y, this.OldAMGFusedMagnet.Value.z);
}
}
else
{
accFilteredPoint = new MCvPoint3D64f(rawAccPoint.x, rawAccPoint.y, rawAccPoint.z);
magnetFilteredPoint = new MCvPoint3D64f(rawMagnetPoint.x, rawMagnetPoint.y, rawMagnetPoint.z);
}
MCvPoint3D64f resAccPoint = new MCvPoint3D64f(0, 9, 0);
MCvPoint3D64f resMagnetPoint = new MCvPoint3D64f(48, 2, 0);
if(this.OldAMGFusedAcc == null || this.OldAMGFusedMagnet == null)
{
resAccPoint = accFilteredPoint;
resMagnetPoint = magnetFilteredPoint;
}
else
{
if (useGyroscope && useAccMagnet)
{
resAccPoint = MatrixToMCvPoint3D64f(MCvPoint3D64fToMatrix(OldAMGFusedAcc.Value).Mul(gyroDiffMatr).Mul(gyroFilterCoeff).Add(MCvPoint3D64fToMatrix(accFilteredPoint).Mul(1 - gyroFilterCoeff)));
resMagnetPoint = MatrixToMCvPoint3D64f(MCvPoint3D64fToMatrix(OldAMGFusedMagnet.Value).Mul(gyroDiffMatr).Mul(gyroFilterCoeff).Add(MCvPoint3D64fToMatrix(magnetFilteredPoint).Mul(1 - gyroFilterCoeff)));
}
else if (useAccMagnet)
{
resAccPoint = accFilteredPoint;
resMagnetPoint = magnetFilteredPoint;
}
else if(useGyroscope)
{
resAccPoint = MatrixToMCvPoint3D64f(MCvPoint3D64fToMatrix(OldAMGFusedAcc.Value).Mul(gyroDiffMatr));
resMagnetPoint = MatrixToMCvPoint3D64f(MCvPoint3D64fToMatrix(OldAMGFusedMagnet.Value).Mul(gyroDiffMatr));
}
}
this.OldAMGFusedAcc = resAccPoint;
this.OldAMGFusedMagnet = resMagnetPoint;
this.OldRawAcc = rawAccPoint;
this.OldRawMagnet = rawMagnetPoint;
this.OldFilteredAcc = accFilteredPoint;
this.OldFilteredMagnet = magnetFilteredPoint;
this.OldGyroReadings = newReadings.GyroVector3f;
MCvPoint3D64f x;
MCvPoint3D64f y;
MCvPoint3D64f z;
this.GetOrthoNormalAccMagnetBasis(resAccPoint, resMagnetPoint, out x, out y, out z);
Matrix<double> accMagnetMatrix = new Matrix<double>(3, 3);
accMagnetMatrix[0, 0] = x.x; accMagnetMatrix[0, 1] = x.y; accMagnetMatrix[0, 2] = x.z;
accMagnetMatrix[1, 0] = y.x; accMagnetMatrix[1, 1] = y.y; accMagnetMatrix[1, 2] = y.z;
accMagnetMatrix[2, 0] = z.x; accMagnetMatrix[2, 1] = z.y; accMagnetMatrix[2, 2] = z.z;
//AccMagnetGyro fuse
Matrix<double> newGAMFusedMatrix = null;
//old amg fusion
//if (this.OldGAMFusedMatrix == null)
//{
// newGAMFusedMatrix = accMagnetMatrix;
//}
//else
//{
// var newGyroMatrix = this.OldGAMFusedMatrix.Mul(gyroDiffMatr);
// newGAMFusedMatrix = this.FuseAccMagnetGyro(accMagnetMatrix, newGyroMatrix, gyroFilterCoeff);
//}
newGAMFusedMatrix = accMagnetMatrix;
OldGAMFusedMatrix = newGAMFusedMatrix;
////
var resMatrix = newGAMFusedMatrix.Transpose();
//res = this.ConvertMatrix(m);
//gyro test
//m = this.OldGyroMatrix;
//m = m.Transpose();
//
//auto res set up
//for (int i = 0; i < 3; ++i)
//{
// for (int j = 0; j < 3; ++j)
// {
// res[i][j] = resMatrix[i, j];
// }
//}
////
//manual res set up
res[0][0] = -resMatrix[0, 0]; res[0][1] = -resMatrix[0, 1]; res[0][2] = resMatrix[0, 2];
res[1][0] = -resMatrix[1, 0]; res[1][1] = -resMatrix[1, 1]; res[1][2] = resMatrix[1, 2];
res[2][0] = -resMatrix[2, 0]; res[2][1] = -resMatrix[2, 1]; res[2][2] = resMatrix[2, 2];
////
return res;
}
private double[] MulReadingsVect(double[][] orientMatix, ReadingsVector3f readings, bool normalize = false)
{
Matrix<double> m = new Matrix<double>(3, 3);
Matrix<double> v = new Matrix<double>(3, 1);
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
m.Data[i, j] = orientMatix[i][j];
}
}
for (int i = 0; i < 3; ++i)
{
v.Data[i, 0] = readings.Values[i];
}
var res = m.Transpose().Mul(v);
if (normalize)
{
var norm = res.Norm;
for (int i = 0; i < 3; ++i)
{
res.Data[i, 0] /= norm;
}
}
return new double[] { res.Data[0, 0], res.Data[1, 0], res.Data[2, 0] };
}
