Finish 0.2.8

README.md

@@ -14,11 +14,11 @@ The test coverage for the package `us.ihmc.ekf.filter`
 
 To use the release of this package add the following to your gradle dependencies:
 
-`compile group: "us.ihmc", name: "ekf", version: "0.2.6"`
+`compile group: "us.ihmc", name: "ekf", version: "0.2.8"`
 
-`compile group: "us.ihmc", name: "ekf-test", version: "0.2.6"`
+`compile group: "us.ihmc", name: "ekf-test", version: "0.2.8"`
 
-`compile group: "us.ihmc", name: "ekf-visualizers", version: "0.2.6"`
+`compile group: "us.ihmc", name: "ekf-visualizers", version: "0.2.8"`
 
 Note, that usually you will only need the main dependency. The others will provide you with tests and some example simulations.
 
@@ -43,6 +43,7 @@ A good introductory paper for Kalman filters can be found [here](https://www.cs.
  - Linear Body Velocity (e.g. Fixed Points, Cameras)
  - Angular Body Velocity (e.g. IMU Sensors)
  - Linear Body Acceleration (e.g. IMU Sensors)
+ - Heading (e.g. Magnetometer in IMU Sensors)
 
 ### Supported States
 

build.gradle

@@ -1,12 +1,12 @@
 plugins {
    id("us.ihmc.ihmc-build") version "0.15.7"
-   id("us.ihmc.ihmc-ci") version "4.23"
+   id("us.ihmc.ihmc-ci") version "4.25"
    id("us.ihmc.ihmc-cd") version "0.1"
 }
 
 ihmc {
    group = "us.ihmc"
-   version = "0.2.7"
+   version = "0.2.8"
    openSource = true
 
    configureDependencyResolution()

src/main/java/us/ihmc/ekf/filter/FilterTools.java

@@ -29,7 +29,7 @@ public static enum ProccessNoiseModel
       ONLY_ACCELERATION_VARIANCE
    }
 
-   public static ProccessNoiseModel proccessNoiseModel = ProccessNoiseModel.PIECEWISE_CONTINUOUS_ACCELERATION;
+   public static ProccessNoiseModel proccessNoiseModel = ProccessNoiseModel.ONLY_ACCELERATION_VARIANCE;
 
    /**
     * This method provides the functionality to convert a velocity jacobian into the overall filter

src/main/java/us/ihmc/ekf/filter/sensor/implementations/MagneticFieldSensor.java

@@ -0,0 +1,150 @@
+package us.ihmc.ekf.filter.sensor.implementations;
+
+import org.ejml.data.DenseMatrix64F;
+import org.ejml.ops.CommonOps;
+
+import us.ihmc.ekf.filter.FilterTools;
+import us.ihmc.ekf.filter.RobotState;
+import us.ihmc.ekf.filter.sensor.Sensor;
+import us.ihmc.euclid.matrix.Matrix3D;
+import us.ihmc.euclid.referenceFrame.FrameVector3D;
+import us.ihmc.euclid.referenceFrame.ReferenceFrame;
+import us.ihmc.euclid.transform.RigidBodyTransform;
+import us.ihmc.euclid.tuple3D.Vector3D;
+import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly;
+import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics;
+import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
+import us.ihmc.mecano.tools.MultiBodySystemTools;
+import us.ihmc.yoVariables.providers.DoubleProvider;
+import us.ihmc.yoVariables.registry.YoVariableRegistry;
+
+/**
+ * A {@link Sensor} implementation for a magnetic field sensor. This is a sensor commonly found in
+ * IMUs that provides the direction of the magnetic field vector at the sensor in sensor
+ * coordinates. To use this sensor call {@link #setMeasurement(Vector3DReadOnly)} once every tick
+ * with the most recent measured value.
+ * <p>
+ * Note, that is class may also be used for any other sensor that provides some kind of heading of
+ * the robot in world frame. The expected measurement direction in world can be calibrated via the
+ * {@link #setNorth(FrameVector3D)} method. By default "north" is assumed to be the x-axis of the
+ * world frame.
+ *
+ * @author Georg Wiedebach
+ *
+ */
+public class MagneticFieldSensor extends Sensor
+{
+   private final String sensorName;
+
+   private final double sqrtHz;
+   private final DoubleProvider variance;
+
+   private final ReferenceFrame measurementFrame;
+   private final Vector3D measurement = new Vector3D();
+   private final FrameVector3D north = new FrameVector3D(ReferenceFrame.getWorldFrame(), 1.0, 0.0, 0.0);
+
+   private final FrameVector3D expectedMeasurement = new FrameVector3D();
+
+   private final RigidBodyBasics measurementBody;
+   private final Matrix3D orientationJacobian = new Matrix3D();
+   private final RigidBodyTransform rootToMeasurement = new RigidBodyTransform();
+   private final RigidBodyTransform rootTransform = new RigidBodyTransform();
+
+   public MagneticFieldSensor(String sensorName, double dt, RigidBodyBasics measurementBody, ReferenceFrame measurementFrame, YoVariableRegistry registry)
+   {
+      this.sensorName = sensorName;
+      this.measurementFrame = measurementFrame;
+      this.measurementBody = measurementBody;
+      sqrtHz = 1.0 / Math.sqrt(dt);
+      variance = FilterTools.findOrCreate(sensorName + "Variance", registry, 1.0);
+   }
+
+   /**
+    * Allows to calibrate this heading sensor. The estimator will attempt to align the vector
+    * provided here with the measured magnetic field direction. By default north is assumed to be
+    * the x-axis of the world frame.
+    *
+    * @param magneticFieldDirectionInWorld
+    *           the direction of the measured magnetic field in the world frame
+    */
+   public void setNorth(FrameVector3D magneticFieldDirectionInWorld)
+   {
+      magneticFieldDirectionInWorld.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());
+      north.setAndNormalize(magneticFieldDirectionInWorld);
+   }
+
+   /**
+    * Set the measurement of this sensor. This should be called once per estimation tick before the
+    * estimate is corrected based on the most recent measurement.
+    *
+    * @param magneticFieldDirection
+    *           the measured magnetic field direction in measurement frame
+    */
+   public void setMeasurement(Vector3DReadOnly magneticFieldDirection)
+   {
+      measurement.setAndNormalize(magneticFieldDirection);
+   }
+
+   @Override
+   public String getName()
+   {
+      return sensorName;
+   }
+
+   @Override
+   public int getMeasurementSize()
+   {
+      return 3;
+   }
+
+   @Override
+   public void getMeasurementJacobian(DenseMatrix64F jacobianToPack, RobotState robotState)
+   {
+      // Could use this to also correct the joint angles. For now only use for base orientation.
+      if (!robotState.isFloating())
+         throw new RuntimeException("This sensor is currently only supported for floating robots.");
+
+      jacobianToPack.reshape(getMeasurementSize(), robotState.getSize());
+      CommonOps.fill(jacobianToPack, 0.0);
+
+      computeExpectedMeasurement();
+
+      RigidBodyBasics rootBody = MultiBodySystemTools.getRootBody(measurementBody);
+      JointBasics rootJoint = rootBody.getChildrenJoints().get(0);
+      ReferenceFrame rootFrame = rootJoint.getFrameAfterJoint();
+      ReferenceFrame baseFrame = rootJoint.getFrameBeforeJoint();
+      rootFrame.getTransformToDesiredFrame(rootToMeasurement, measurementFrame);
+      baseFrame.getTransformToDesiredFrame(rootTransform, rootFrame);
+
+      orientationJacobian.setToTildeForm(expectedMeasurement);
+      orientationJacobian.multiply(rootToMeasurement.getRotationMatrix());
+      orientationJacobian.multiply(rootTransform.getRotationMatrix());
+      orientationJacobian.get(0, robotState.findOrientationIndex(), jacobianToPack);
+   }
+
+   @Override
+   public void getResidual(DenseMatrix64F residualToPack, RobotState robotState)
+   {
+      computeExpectedMeasurement();
+
+      residualToPack.reshape(getMeasurementSize(), 1);
+      residualToPack.set(0, measurement.getX() - expectedMeasurement.getX());
+      residualToPack.set(1, measurement.getY() - expectedMeasurement.getY());
+      residualToPack.set(2, measurement.getZ() - expectedMeasurement.getZ());
+   }
+
+   private void computeExpectedMeasurement()
+   {
+      expectedMeasurement.setIncludingFrame(north);
+      expectedMeasurement.changeFrame(measurementFrame);
+   }
+
+   @Override
+   public void getRMatrix(DenseMatrix64F noiseCovarianceToPack)
+   {
+      noiseCovarianceToPack.reshape(getMeasurementSize(), getMeasurementSize());
+      CommonOps.setIdentity(noiseCovarianceToPack);
+      CommonOps.scale(variance.getValue() * sqrtHz, noiseCovarianceToPack);
+   }
+
+}

src/test/java/us/ihmc/ekf/ImuOrientationEstimatorWithMagnetometerTest.java

@@ -0,0 +1,132 @@
+package us.ihmc.ekf;
+
+import java.util.Arrays;
+import java.util.Collections;
+import java.util.List;
+import java.util.Random;
+
+import org.junit.jupiter.api.Test;
+
+import us.ihmc.ekf.filter.RobotState;
+import us.ihmc.ekf.filter.StateEstimator;
+import us.ihmc.ekf.filter.sensor.Sensor;
+import us.ihmc.ekf.filter.sensor.implementations.AngularVelocitySensor;
+import us.ihmc.ekf.filter.sensor.implementations.LinearAccelerationSensor;
+import us.ihmc.ekf.filter.sensor.implementations.LinearVelocitySensor;
+import us.ihmc.ekf.filter.sensor.implementations.MagneticFieldSensor;
+import us.ihmc.ekf.filter.state.implementations.PoseState;
+import us.ihmc.euclid.referenceFrame.FrameQuaternion;
+import us.ihmc.euclid.referenceFrame.FrameVector3D;
+import us.ihmc.euclid.referenceFrame.ReferenceFrame;
+import us.ihmc.euclid.referenceFrame.tools.EuclidFrameRandomTools;
+import us.ihmc.euclid.tools.EuclidCoreRandomTools;
+import us.ihmc.euclid.tools.EuclidCoreTestTools;
+import us.ihmc.euclid.transform.RigidBodyTransform;
+import us.ihmc.euclid.tuple3D.Vector3D;
+import us.ihmc.mecano.frames.MovingReferenceFrame;
+import us.ihmc.mecano.multiBodySystem.RigidBody;
+import us.ihmc.mecano.multiBodySystem.SixDoFJoint;
+import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
+import us.ihmc.mecano.multiBodySystem.interfaces.SixDoFJointBasics;
+import us.ihmc.mecano.spatial.Twist;
+import us.ihmc.mecano.tools.MecanoRandomTools;
+import us.ihmc.yoVariables.parameters.DefaultParameterReader;
+import us.ihmc.yoVariables.registry.YoVariableRegistry;
+
+public class ImuOrientationEstimatorWithMagnetometerTest
+{
+   private static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
+   private static final double ESTIMATOR_DT = 0.001;
+   private static final Random random = new Random(2549862L);
+
+   @Test
+   public void testFullOrientationEstimation()
+   {
+      for (int i = 0; i < 10; i++)
+      {
+         testOnce();
+      }
+   }
+
+   public void testOnce()
+   {
+      YoVariableRegistry registry = new YoVariableRegistry("TestRegistry");
+
+      // Create ID structure with a single floating joint and a IMU attached to it.
+      RigidBodyBasics elevator = new RigidBody("elevator", worldFrame);
+      SixDoFJoint joint = new SixDoFJoint("joint", elevator);
+      RigidBodyBasics body = new RigidBody("body", joint, 0.1, 0.1, 0.1, 1.0, new Vector3D());
+      RigidBodyTransform imuTransform = EuclidCoreRandomTools.nextRigidBodyTransform(random);
+      MovingReferenceFrame measurementFrame = MovingReferenceFrame.constructFrameFixedInParent("MeasurementFrame", joint.getFrameAfterJoint(), imuTransform);
+
+      // Make the sensors!
+      LinearAccelerationSensor linearAccelerationSensor = new LinearAccelerationSensor("Accelerometer", ESTIMATOR_DT, body, measurementFrame, false, registry);
+      MagneticFieldSensor magneticFieldSensor = new MagneticFieldSensor("Magnetometer", ESTIMATOR_DT, body, measurementFrame, registry);
+      AngularVelocitySensor angularVelocitySensor = new AngularVelocitySensor("Gyroscope", ESTIMATOR_DT, body, measurementFrame, false, registry);
+      LinearVelocitySensor linearVelocitySensor = new LinearVelocitySensor("LinearVelocity", ESTIMATOR_DT, body, measurementFrame, false, registry);
+      List<Sensor> sensors = Arrays.asList(new Sensor[] {angularVelocitySensor, linearVelocitySensor, linearAccelerationSensor, magneticFieldSensor});
+
+      PoseState poseState = new PoseState("Body", ESTIMATOR_DT, joint.getFrameAfterJoint(), registry);
+      RobotState robotState = new RobotState(poseState, Collections.emptyList());
+      StateEstimator estimator = new StateEstimator(sensors, robotState, registry);
+
+      // Start with a random joint pose and twist estimate
+      RigidBodyTransform initialTransform = EuclidCoreRandomTools.nextRigidBodyTransform(random);
+      Twist initialTwist = MecanoRandomTools.nextTwist(random, joint.getFrameAfterJoint(), joint.getFrameBeforeJoint(), joint.getFrameAfterJoint());
+      poseState.initialize(initialTransform, initialTwist);
+
+      // Create a random direction in the xy plane to be "north"
+      FrameVector3D zAxis = new FrameVector3D(worldFrame, 0.0, 0.0, 1.0);
+      FrameVector3D north = EuclidFrameRandomTools.nextOrthogonalFrameVector3D(random, zAxis, false);
+      magneticFieldSensor.setNorth(north);
+
+      // Set fake measurements based on some orientation
+      FrameQuaternion expectedOrientation = EuclidFrameRandomTools.nextFrameQuaternion(random, worldFrame);
+      RigidBodyTransform expectedRootTransform = new RigidBodyTransform(expectedOrientation, new FrameVector3D());
+
+      // Create measurements in world then transform it to the root joint and finally to the imu orientation
+      FrameVector3D gravity = new FrameVector3D(zAxis);
+      gravity.scale(-RobotState.GRAVITY);
+      gravity.applyInverseTransform(expectedRootTransform);
+      gravity.applyInverseTransform(imuTransform);
+      linearAccelerationSensor.setMeasurement(gravity);
+      FrameVector3D magneticField = new FrameVector3D(north);
+      magneticField.applyInverseTransform(expectedRootTransform);
+      magneticField.applyInverseTransform(imuTransform);
+      magneticFieldSensor.setMeasurement(magneticField);
+
+      loadParameters(registry);
+
+      FrameQuaternion actualOrientation = new FrameQuaternion();
+      for (int i = 0; i < 20000; i++)
+      {
+         estimator.predict();
+         updateJoint(joint, poseState);
+
+         estimator.correct();
+         updateJoint(joint, poseState);
+
+         poseState.getOrientation(actualOrientation);
+      }
+
+      EuclidCoreTestTools.assertQuaternionGeometricallyEquals(expectedOrientation, actualOrientation, 1.0e-4);
+   }
+
+   private static void updateJoint(SixDoFJointBasics joint, PoseState poseState)
+   {
+      RigidBodyTransform jointTransform = new RigidBodyTransform();
+      poseState.getTransform(jointTransform);
+      joint.setJointConfiguration(jointTransform);
+
+      Twist jointTwist = new Twist();
+      poseState.getTwist(jointTwist);
+      joint.setJointTwist(jointTwist);
+
+      joint.updateFramesRecursively();
+   }
+
+   private static void loadParameters(YoVariableRegistry registry)
+   {
+      new DefaultParameterReader().readParametersInRegistry(registry);
+   }
+}