예제 #1
0
        public /*synchronized*/ void processAcc(float[] acc, long sensorTimeStamp)
        {
            mz.set(acc[0], acc[1], acc[2]);

            if (sensorTimeStampAcc != 0L)
            {
                accObservationFunctionForNumericalJacobian(so3SensorFromWorld, mNu);

                double eps = 1.0E-07D;
                for (int dof = 0; dof < 3; dof++)
                {
                    Vector3d delta = processAccVDelta;
                    delta.setZero();
                    delta.setComponent(dof, eps);

                    So3Util.sO3FromMu(delta, processAccTempM1);
                    Matrix3x3d.mult(processAccTempM1, so3SensorFromWorld, processAccTempM2);

                    accObservationFunctionForNumericalJacobian(processAccTempM2, processAccTempV1);

                    Vector3d withDelta = processAccTempV1;

                    Vector3d.sub(mNu, withDelta, processAccTempV2);
                    processAccTempV2.scale(1.0D / eps);
                    mH.setColumn(dof, processAccTempV2);
                }

                mH.transpose(processAccTempM3);
                Matrix3x3d.mult(mP, processAccTempM3, processAccTempM4);
                Matrix3x3d.mult(mH, processAccTempM4, processAccTempM5);
                Matrix3x3d.add(processAccTempM5, mRaccel, mS);

                mS.invert(processAccTempM3);
                mH.transpose(processAccTempM4);
                Matrix3x3d.mult(processAccTempM4, processAccTempM3, processAccTempM5);
                Matrix3x3d.mult(mP, processAccTempM5, mK);

                Matrix3x3d.mult(mK, mNu, mx);

                Matrix3x3d.mult(mK, mH, processAccTempM3);
                processAccTempM4.setIdentity();
                processAccTempM4.minusEquals(processAccTempM3);
                Matrix3x3d.mult(processAccTempM4, mP, processAccTempM3);
                mP.set(processAccTempM3);

                So3Util.sO3FromMu(mx, so3LastMotion);

                Matrix3x3d.mult(so3LastMotion, so3SensorFromWorld, so3SensorFromWorld);

                updateCovariancesAfterMotion();
            }
            else
            {
                So3Util.sO3FromTwoVec(down, mz, so3SensorFromWorld);
            }
            sensorTimeStampAcc = sensorTimeStamp;
        }
예제 #2
0
        public void processMag(float[] mag, long sensorTimeStamp)
        {
            mz.set(mag[0], mag[1], mag[2]);
            mz.normalize();

            Vector3d downInSensorFrame = new Vector3d();

            so3SensorFromWorld.getColumn(2, downInSensorFrame);

            Vector3d.cross(mz, downInSensorFrame, processMagTempV1);
            Vector3d perpToDownAndMag = processMagTempV1;

            perpToDownAndMag.normalize();

            Vector3d.cross(downInSensorFrame, perpToDownAndMag, processMagTempV2);
            Vector3d magHorizontal = processMagTempV2;

            magHorizontal.normalize();
            mz.set(magHorizontal);

            if (sensorTimeStampMag != 0L)
            {
                magObservationFunctionForNumericalJacobian(so3SensorFromWorld, mNu);

                double eps = 1.0E-07D;
                for (int dof = 0; dof < 3; dof++)
                {
                    Vector3d delta = processMagTempV3;
                    delta.setZero();
                    delta.setComponent(dof, eps);

                    So3Util.sO3FromMu(delta, processMagTempM1);
                    Matrix3x3d.mult(processMagTempM1, so3SensorFromWorld, processMagTempM2);

                    magObservationFunctionForNumericalJacobian(processMagTempM2, processMagTempV4);

                    Vector3d withDelta = processMagTempV4;

                    Vector3d.sub(mNu, withDelta, processMagTempV5);
                    processMagTempV5.scale(1.0D / eps);

                    mH.setColumn(dof, processMagTempV5);
                }

                mH.transpose(processMagTempM4);
                Matrix3x3d.mult(mP, processMagTempM4, processMagTempM5);
                Matrix3x3d.mult(mH, processMagTempM5, processMagTempM6);
                Matrix3x3d.add(processMagTempM6, mR, mS);

                mS.invert(processMagTempM4);
                mH.transpose(processMagTempM5);
                Matrix3x3d.mult(processMagTempM5, processMagTempM4, processMagTempM6);
                Matrix3x3d.mult(mP, processMagTempM6, mK);

                Matrix3x3d.mult(mK, mNu, mx);

                Matrix3x3d.mult(mK, mH, processMagTempM4);
                processMagTempM5.setIdentity();
                processMagTempM5.minusEquals(processMagTempM4);
                Matrix3x3d.mult(processMagTempM5, mP, processMagTempM4);
                mP.set(processMagTempM4);

                So3Util.sO3FromMu(mx, so3LastMotion);

                Matrix3x3d.mult(so3LastMotion, so3SensorFromWorld, processMagTempM4);
                so3SensorFromWorld.set(processMagTempM4);

                updateCovariancesAfterMotion();
            }
            else
            {
                magObservationFunctionForNumericalJacobian(so3SensorFromWorld, mNu);
                So3Util.sO3FromMu(mx, so3LastMotion);

                Matrix3x3d.mult(so3LastMotion, so3SensorFromWorld, processMagTempM4);
                so3SensorFromWorld.set(processMagTempM4);

                updateCovariancesAfterMotion();
            }
            sensorTimeStampMag = sensorTimeStamp;
        }