コード例 #1
0
ファイル: ReadingsVector3f.cs プロジェクト: JimmHub/odoStuff
 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;
 }
コード例 #2
0
        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;
        }
コード例 #3
0
        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;
        }
コード例 #4
0
        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;
        }
コード例 #5
0
        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;
        }
コード例 #6
0
        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;
        }
コード例 #7
0
ファイル: Form1.cs プロジェクト: JimmHub/odoStuff
        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] };
        }