diff --git a/ihmc-parameter-estimation/src/main/java/us/ihmc/parameterEstimation/ExtendedKalmanFilter.java b/ihmc-parameter-estimation/src/main/java/us/ihmc/parameterEstimation/ExtendedKalmanFilter.java
index 35ce3cc8dee1..9a9a1cbe01d7 100644
--- a/ihmc-parameter-estimation/src/main/java/us/ihmc/parameterEstimation/ExtendedKalmanFilter.java
+++ b/ihmc-parameter-estimation/src/main/java/us/ihmc/parameterEstimation/ExtendedKalmanFilter.java
@@ -6,6 +6,7 @@
import org.ejml.dense.row.CommonOps_DDRM;
import org.ejml.dense.row.MatrixFeatures_DDRM;
import org.ejml.dense.row.factory.LinearSolverFactory_DDRM;
+import us.ihmc.matrixlib.MatrixTools;
/**
* This class provides an implementation of an Extended Kalman Filter (EKF). The EKF is a recursive filter that uses a linearization of the process and
@@ -13,6 +14,22 @@
*
* This implementation is for state-only systems, no input -- or if you prefer: x_dot = f(x), not x_dot = f(x, u).
*
+ *
+ * To utilize this filter, the user must extend this class and provide implementations of the process and measurement models by overriding the abstract methods:
+ *
+ * - {@link #processModel(DMatrixRMaj)}
+ * - {@link #measurementModel(DMatrixRMaj)}
+ *
+ * Then, to get updates, the user calls {@link #calculateEstimate(DMatrix)} with the observation at the current time step. This will return the estimated
+ * state based on the observations.
+ *
+ *
+ * For example implementations, see:
+ *
+ * - {@link us.ihmc.parameterEstimation.examples.ExamplePendulumExtendedKalmanFilter}
+ * - {@link us.ihmc.parameterEstimation.examples.ExampleConstantVelocity2DKalmanFilter}
+ *
+ *
*
* @author James Foster
*/
@@ -66,7 +83,7 @@ public abstract class ExtendedKalmanFilter
/**
* The residual 'y' between the actual observation (i.e. direct from the sensors), and the predicted measurement from the prediction step of the filter.
*/
- private final DMatrixRMaj measurementResidual;
+ protected final DMatrixRMaj measurementResidual;
/**
* Container variable used for garbage free operations.
*/
@@ -74,7 +91,23 @@ public abstract class ExtendedKalmanFilter
/**
* Covariance matrix 'S' of the linearized measurement model, used in the update step of the filter.
*/
- private final DMatrixRMaj residualCovariance;
+ protected final DMatrixRMaj residualCovariance;
+ /**
+ * Masked version of the residual, mask^T * y
+ */
+ protected final DMatrixRMaj maskedMeasurementResidual;
+ /**
+ * Masked version of the measurement covariance, mask^T * R * mask, used to reject outlier measurements from the filter
+ */
+ protected final DMatrixRMaj maskedMeasurementCovariance;
+ /**
+ * Masked version of the covariance, mask^T * S * mask, used to reject outlier measurements from the filter
+ */
+ protected final DMatrixRMaj maskedResidualCovariance;
+ /**
+ * Masked measurement jacobian, mask^T * H
+ */
+ protected final DMatrixRMaj maskedMeasurementJacobian;
/**
* Container variable used to hold the inverse of the residual covariance matrix.
*/
@@ -92,7 +125,10 @@ public abstract class ExtendedKalmanFilter
* Container variable used for garbage free operations.
*/
private final DMatrixRMaj kalmanGainContainer;
-
+ /**
+ * This is the update to the predicted state, based on the Kalman feedback.
+ */
+ protected final DMatrixRMaj stateCorrection;
/**
* The predicted estimate of the state according to the measurement model of the filter.
*/
@@ -123,32 +159,47 @@ public abstract class ExtendedKalmanFilter
/** Terms used for calculating the Joseph form of the covariance update. */
private final DMatrixRMaj josephTransposeTerm;
- private final DMatrixRMaj josephTransposeTermContainer;
- private final DMatrixRMaj josephIdentity;
- private final DMatrixRMaj josephGainTerm;
private final DMatrixRMaj josephGainTermContainer;
+ private final int stateSize;
+ private final int errorSize;
+ private final int measurementSize;
+
public ExtendedKalmanFilter(int stateSize, int measurementSize)
{
- processCovariance = new DMatrixRMaj(stateSize, stateSize);
+ this(stateSize, stateSize, measurementSize);
+ }
+
+ public ExtendedKalmanFilter(int stateSize, int errorSize, int measurementSize)
+ {
+ this.stateSize = stateSize;
+ this.errorSize = errorSize;
+ this.measurementSize = measurementSize;
+
+ processCovariance = new DMatrixRMaj(errorSize, errorSize);
measurementCovariance = new DMatrixRMaj(measurementSize, measurementSize);
+ maskedMeasurementCovariance = new DMatrixRMaj(measurementSize, measurementSize);
- processJacobian = new DMatrixRMaj(stateSize, stateSize);
- measurementJacobian = new DMatrixRMaj(measurementSize, stateSize);
+ processJacobian = new DMatrixRMaj(stateSize, errorSize);
+ measurementJacobian = new DMatrixRMaj(measurementSize, errorSize);
+ maskedMeasurementJacobian = new DMatrixRMaj(measurementSize, errorSize);
state = new DMatrixRMaj(stateSize, 1);
predictedState = new DMatrixRMaj(stateSize, 1);
- covariance = new DMatrixRMaj(stateSize, stateSize);
- predictedCovariance = new DMatrixRMaj(stateSize, stateSize);
- predictedCovarianceContainer = new DMatrixRMaj(stateSize, stateSize);
+ covariance = new DMatrixRMaj(errorSize, errorSize);
+ predictedCovariance = new DMatrixRMaj(errorSize, errorSize);
+ predictedCovarianceContainer = new DMatrixRMaj(errorSize, errorSize);
measurementResidual = new DMatrixRMaj(measurementSize, 1);
- residualCovarianceContainer = new DMatrixRMaj(measurementSize, measurementSize);
+ maskedMeasurementResidual = new DMatrixRMaj(measurementSize, 1);
+ residualCovarianceContainer = new DMatrixRMaj(errorSize, errorSize);
residualCovariance = new DMatrixRMaj(measurementSize, measurementSize);
+ maskedResidualCovariance = new DMatrixRMaj(measurementSize, measurementSize);
inverseResidualCovariance = new DMatrixRMaj(measurementSize, measurementSize);
solver = new LinearSolverSafe<>(LinearSolverFactory_DDRM.symmPosDef(measurementSize));
kalmanGain = new DMatrixRMaj(stateSize, measurementSize);
kalmanGainContainer = new DMatrixRMaj(stateSize, measurementSize);
+ stateCorrection = new DMatrixRMaj(errorSize, 1);
updatedState = new DMatrixRMaj(stateSize, 1);
updatedCovariance = new DMatrixRMaj(stateSize, stateSize);
updatedCovarianceContainer = new DMatrixRMaj(stateSize, stateSize);
@@ -156,11 +207,8 @@ public ExtendedKalmanFilter(int stateSize, int measurementSize)
normalizedInnovation = new DMatrixRMaj(measurementSize, 1);
normalizedInnovationContainer = new DMatrixRMaj(measurementSize, 1);
- josephTransposeTerm = new DMatrixRMaj(stateSize, stateSize);
- josephTransposeTermContainer = new DMatrixRMaj(stateSize, stateSize);
- josephIdentity = CommonOps_DDRM.identity(stateSize);
- josephGainTerm = new DMatrixRMaj(stateSize, stateSize);
- josephGainTermContainer = new DMatrixRMaj(stateSize, stateSize);
+ josephTransposeTerm = new DMatrixRMaj(errorSize, errorSize);
+ josephGainTermContainer = new DMatrixRMaj(errorSize, errorSize);
}
public ExtendedKalmanFilter(DMatrixRMaj initialState, DMatrixRMaj initialCovariance, DMatrixRMaj processCovariance, DMatrixRMaj measurementCovariance)
@@ -188,6 +236,26 @@ public ExtendedKalmanFilter(DMatrixRMaj initialState, DMatrixRMaj initialCovaria
covariance.set(initialCovariance);
}
+ public void initializeState(DMatrixRMaj state)
+ {
+ this.state.set(state);
+ }
+
+ public int getStateSize()
+ {
+ return stateSize;
+ }
+
+ public int getErrorSize()
+ {
+ return errorSize;
+ }
+
+ public int getMeasurementSize()
+ {
+ return measurementSize;
+ }
+
/**
* Runs the filter for a single time step.
*
@@ -280,6 +348,15 @@ private void applyInnovationGate()
covariance.set(predictedCovariance);
}
+ /**
+ * This is broken into its own function, in case the state exists on some manifold. This is the case for, for example, quaternions. In that event, this
+ * function should overriden.
+ */
+ protected void applyStateCorrection(DMatrixRMaj state, DMatrixRMaj modification, DMatrixRMaj modifiedState)
+ {
+ CommonOps_DDRM.add(state, modification, modifiedState);
+ }
+
/**
* If the observation is valid, the actual observation, as well as the measurement model and its noise profile, are used to correct the
* prediction made in the {@link #predictionStep()}.
@@ -289,8 +366,9 @@ protected void updateStep()
calculateKalmanGain();
// x_(k|k) = x_(k|k-1) + K_k * y_k
- updatedState.set(predictedState);
- CommonOps_DDRM.multAdd(kalmanGain, measurementResidual, updatedState);
+ CommonOps_DDRM.mult(kalmanGain, maskedMeasurementResidual, stateCorrection);
+ // We want to do this, in case the state exists on a manifold (e.g. quaternion)
+ applyStateCorrection(predictedState, stateCorrection, updatedState);
calculateUpdatedCovariance();
state.set(updatedState);
@@ -300,20 +378,33 @@ protected void updateStep()
private void calculateResidualCovarianceAndInverse()
{
// S_k = H_k * P_(k|k-1) * H_k^T + R_k
- residualCovariance.zero();
CommonOps_DDRM.mult(measurementJacobian, predictedCovariance, residualCovarianceContainer);
CommonOps_DDRM.multTransB(residualCovarianceContainer, measurementJacobian, residualCovariance);
CommonOps_DDRM.addEquals(residualCovariance, measurementCovariance);
- solver.setA(residualCovariance);
- solver.invert(inverseResidualCovariance);
+ maskResidualCovarianceForOutlierRejection();
+
+ solver.setA(maskedResidualCovariance);
+ // fixme this doesn't work.
+ CommonOps_DDRM.invert(maskedResidualCovariance, inverseResidualCovariance);
+// solver.invert(inverseResidualCovariance);
+ }
+
+ /**
+ * If the user wants to reject outlier measurements, they can override this method to modify the masked variables. By default, no masking is done.
+ */
+ protected void maskResidualCovarianceForOutlierRejection()
+ {
+ maskedResidualCovariance.set(residualCovariance);
+ maskedMeasurementCovariance.set(measurementCovariance);
+ maskedMeasurementResidual.set(measurementResidual);
+ maskedMeasurementJacobian.set(measurementJacobian);
}
protected void calculateKalmanGain()
{
// K_k = P_(k|k-1) * H_k^T * S_k^-1
- kalmanGain.zero();
- CommonOps_DDRM.multTransB(predictedCovariance, measurementJacobian, kalmanGainContainer);
+ CommonOps_DDRM.multTransB(predictedCovariance, maskedMeasurementJacobian, kalmanGainContainer);
CommonOps_DDRM.mult(kalmanGainContainer, inverseResidualCovariance, kalmanGain);
}
@@ -324,28 +415,22 @@ protected void calculateUpdatedCovariance()
{
// Joseph form of covariance update is apparently more numerically stable
// P_(k|k) = (I - K_k * H_k) * P_(k|k-1) * (I - K_k * H_k)^T + K_k * R_k * K_k^T
- josephGainTerm.set(kalmanGain);
- CommonOps_DDRM.mult(josephGainTerm, measurementCovariance, josephGainTermContainer);
- CommonOps_DDRM.multTransB(josephGainTermContainer, kalmanGain, josephGainTerm);
+ CommonOps_DDRM.mult(kalmanGain, maskedMeasurementCovariance, josephGainTermContainer);
+ CommonOps_DDRM.multTransB(josephGainTermContainer, kalmanGain, updatedCovariance);
- josephTransposeTermContainer.set(kalmanGain);
- CommonOps_DDRM.mult(josephTransposeTermContainer, measurementJacobian, josephTransposeTerm);
- CommonOps_DDRM.scale(-1.0, josephTransposeTerm);
- CommonOps_DDRM.addEquals(josephTransposeTerm, josephIdentity);
+ CommonOps_DDRM.mult(-1.0, kalmanGain, maskedMeasurementJacobian, josephTransposeTerm);
+ MatrixTools.addDiagonal(josephTransposeTerm, 1.0);
// Now put these terms into the covariance update variables
CommonOps_DDRM.multTransB(predictedCovariance, josephTransposeTerm, updatedCovarianceContainer);
- CommonOps_DDRM.mult(josephTransposeTerm, updatedCovarianceContainer, updatedCovariance);
- CommonOps_DDRM.addEquals(updatedCovariance, josephGainTerm);
+ CommonOps_DDRM.multAdd(josephTransposeTerm, updatedCovarianceContainer, updatedCovariance);
}
else // DEFAULT
{
// P_(k|k) = (I - K_k * H_k) * P_(k|k-1)
- updatedCovariance.set(kalmanGain);
- CommonOps_DDRM.mult(updatedCovariance, measurementJacobian, updatedCovarianceContainer);
+ CommonOps_DDRM.mult(-1.0, kalmanGain, measurementJacobian, updatedCovarianceContainer);
+ MatrixTools.addDiagonal(updatedCovarianceContainer, 1.0);
CommonOps_DDRM.mult(updatedCovarianceContainer, predictedCovariance, updatedCovariance);
- CommonOps_DDRM.scale(-1.0, updatedCovariance);
- CommonOps_DDRM.addEquals(updatedCovariance, predictedCovariance);
}
}
@@ -361,8 +446,8 @@ protected void calculateUpdatedCovariance()
*/
private double calculateNormalizedInnovation()
{
- CommonOps_DDRM.mult(inverseResidualCovariance, measurementResidual, normalizedInnovationContainer);
- CommonOps_DDRM.multTransA(measurementResidual, normalizedInnovationContainer, normalizedInnovation);
+ CommonOps_DDRM.mult(inverseResidualCovariance, maskedMeasurementResidual, normalizedInnovationContainer);
+ CommonOps_DDRM.multTransA(maskedMeasurementResidual, normalizedInnovationContainer, normalizedInnovation);
return normalizedInnovation.getData()[0];
}
@@ -424,6 +509,11 @@ public double getNormalizedInnovation()
return normalizedInnovation.getData()[0];
}
+ public DMatrixRMaj getCovariance()
+ {
+ return covariance;
+ }
+
public void setNormalizedInnovationThreshold(double normalizedInnovationThreshold)
{
this.normalizedInnovationThreshold = normalizedInnovationThreshold;
diff --git a/ihmc-parameter-estimation/src/main/java/us/ihmc/parameterEstimation/examples/ExampleConstantVelocity2DKalmanFilter.java b/ihmc-parameter-estimation/src/main/java/us/ihmc/parameterEstimation/examples/ExampleConstantVelocity2DKalmanFilter.java
new file mode 100644
index 000000000000..264adf477c08
--- /dev/null
+++ b/ihmc-parameter-estimation/src/main/java/us/ihmc/parameterEstimation/examples/ExampleConstantVelocity2DKalmanFilter.java
@@ -0,0 +1,74 @@
+package us.ihmc.parameterEstimation.examples;
+
+import org.ejml.data.DMatrixRMaj;
+import org.ejml.dense.row.CommonOps_DDRM;
+import us.ihmc.parameterEstimation.ExtendedKalmanFilter;
+
+public class ExampleConstantVelocity2DKalmanFilter extends ExtendedKalmanFilter
+{
+ static final int stateSize = 4;
+ static final int measurementSize = 2;
+
+ // Start at (x,y) = (0,0) with no velocity
+ static final DMatrixRMaj x0 = new DMatrixRMaj(new double[] {0.0, 0.0, 0.0, 0.0});
+
+ // Position is barely affected by noise, velocity is affected by noise
+ static final DMatrixRMaj Q = new DMatrixRMaj(new double[][] {{1e-6, 0.0, 0.0, 0.0},
+ {0.0, 1e-6, 0.0, 0.0},
+ {0.0, 0.0, 1e-6, 0.0},
+ {0.0, 0.0, 0.0, 1e-6}});
+
+ // Both x and y position measurements are corrupted by noise
+ static final DMatrixRMaj R = new DMatrixRMaj(new double[][] {{1, 0.0}, {0.0, 1}});
+
+ // Ignorant initial guess on P0, we assume we're more certain about positions than velocities
+ static final DMatrixRMaj P0 = new DMatrixRMaj(new double[][] {{0.1, 0.0, 0.0, 0.0},
+ {0.0, 1.0, 0.0, 0.0},
+ {0.0, 0.0, 0.1, 0.0},
+ {0.0, 0.0, 0.0, 1.0}});
+
+ static final double dt = 0.01;
+
+ // Constant velocity model of a 2D planar system
+ static final DMatrixRMaj A = new DMatrixRMaj(new double[][] {{1.0, dt, 0.0, 0.0}, {0.0, 1.0, 0.0, 0.0}, {0.0, 0.0, 1.0, dt}, {0.0, 0.0, 0.0, 1.0}});
+
+ // We only measure positions, not velocities
+ static final DMatrixRMaj C = new DMatrixRMaj(new double[][] {{1.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 1.0, 0.0}});
+
+ public ExampleConstantVelocity2DKalmanFilter()
+ {
+ super(x0, P0, Q, R);
+ }
+
+ // In the case of a linear process model, the linearization is just the A matrix of the process model
+ @Override
+ public DMatrixRMaj linearizeProcessModel(DMatrixRMaj previousState)
+ {
+ return A;
+ }
+
+ // In the case of a linear measurement model, the linearization is just the C matrix of the measurement model
+ @Override
+ public DMatrixRMaj linearizeMeasurementModel(DMatrixRMaj predictedState)
+ {
+ return C;
+ }
+
+ // y = Ax
+ @Override
+ public DMatrixRMaj processModel(DMatrixRMaj state)
+ {
+ DMatrixRMaj nextState = new DMatrixRMaj(stateSize, 1);
+ CommonOps_DDRM.mult(A, state, nextState);
+ return nextState;
+ }
+
+ // y = Cx
+ @Override
+ public DMatrixRMaj measurementModel(DMatrixRMaj state)
+ {
+ DMatrixRMaj measurement = new DMatrixRMaj(measurementSize, 1);
+ CommonOps_DDRM.mult(C, state, measurement);
+ return measurement;
+ }
+}
diff --git a/ihmc-parameter-estimation/src/main/java/us/ihmc/parameterEstimation/examples/ExamplePendulumExtendedKalmanFilter.java b/ihmc-parameter-estimation/src/main/java/us/ihmc/parameterEstimation/examples/ExamplePendulumExtendedKalmanFilter.java
new file mode 100644
index 000000000000..7bc9cf752727
--- /dev/null
+++ b/ihmc-parameter-estimation/src/main/java/us/ihmc/parameterEstimation/examples/ExamplePendulumExtendedKalmanFilter.java
@@ -0,0 +1,136 @@
+package us.ihmc.parameterEstimation.examples;
+
+import org.ejml.data.DMatrixRMaj;
+import us.ihmc.parameterEstimation.ExtendedKalmanFilter;
+
+public class ExamplePendulumExtendedKalmanFilter extends ExtendedKalmanFilter
+{
+ static final int stateSize = 2;
+ static final int measurementSize = 1;
+
+ // Picking an arbitrary nonzero initial condition since 0 is a singularity for the measurement
+ private static final DMatrixRMaj x0 = new DMatrixRMaj(new double[] {0.1, 0.1});
+
+ // Process noise
+ private static final DMatrixRMaj Q = new DMatrixRMaj(new double[][] {{1e-6, 0.0}, {0.0, 1e-6}});
+
+ // Measurement noise
+ private static final DMatrixRMaj R = new DMatrixRMaj(new double[][] {{1e-3}});
+
+ // Ignorant initial guess on P0, we assume we're more certain about positions than velocities
+ private static final DMatrixRMaj P0 = new DMatrixRMaj(new double[][] {{0.1, 0.0}, {0.0, 1.0}});
+
+ // Linearized process model dynamics ( Jacobian of f(x) where x[k+1] = f(x[k]) )
+ final DMatrixRMaj F = new DMatrixRMaj(stateSize, stateSize);
+
+ // Linearized measurement model dynamics (Jacobian of h(x) where y = h(x))
+ final DMatrixRMaj H = new DMatrixRMaj(measurementSize, stateSize);
+
+ private double I = 0.006; // inertia [kg m^2]
+ private double m = 0.2; // mass [kg]
+ private double b = 0.02; // damping coefficient (not pendulum) [N/s]
+ private double g = 9.8; // acceleration due to gravity [m/s^2]
+ private double l = 0.3; // length to center of mass [m]
+ private double dt = 0.01; // integration dt [s]
+
+ public ExamplePendulumExtendedKalmanFilter()
+ {
+ super(x0, P0, Q, R);
+ }
+
+ public void setModelProperties(double m, double I, double l, double b, double g, double dt)
+ {
+ this.m = m;
+ this.I = I;
+ this.l = l;
+ this.b = b;
+ this.g = g;
+ this.dt = dt;
+ }
+
+ public DMatrixRMaj getQ()
+ {
+ return Q;
+ }
+
+ public DMatrixRMaj getR()
+ {
+ return R;
+ }
+
+ public DMatrixRMaj getF()
+ {
+ return F;
+ }
+
+ public DMatrixRMaj getH()
+ {
+ return H;
+ }
+
+ // Linearize f(x) to obtain it's Jacobian matrix, F(x)
+ @Override
+ public DMatrixRMaj linearizeProcessModel(DMatrixRMaj previousState)
+ {
+ double q = previousState.get(0, 0);
+
+ F.set(0, 0, 1);
+ F.set(0, 1, dt);
+ F.set(1, 0, -(dt * m * g * l * Math.cos(q)) / I);
+ F.set(1, 1, 1 - dt * b / I);
+
+ return F;
+ }
+
+ // Linearize h(x) to obtain it's Jacobian matrix, H(x)
+ @Override
+ public DMatrixRMaj linearizeMeasurementModel(DMatrixRMaj predictedState)
+ {
+ double q = predictedState.get(0, 0);
+ double qDot = predictedState.get(1, 0);
+
+ H.set(0, 0, m * g * l * Math.sin(q));
+ H.set(0, 1, I * qDot);
+
+ return H;
+ }
+
+ // x[k+1] = f(x[k])
+ @Override
+ public DMatrixRMaj processModel(DMatrixRMaj state)
+ {
+ return getNextState(state);
+ }
+
+ // y = h(x)
+ @Override
+ public DMatrixRMaj measurementModel(DMatrixRMaj state)
+ {
+ return getMeasurement(state);
+ }
+
+ DMatrixRMaj getNextState(DMatrixRMaj state)
+ {
+ DMatrixRMaj nextState = new DMatrixRMaj(state.numRows, 1);
+ double q = state.get(0, 0);
+ double qDot = state.get(1, 0);
+
+ // Discretized with Forward Euler
+ nextState.set(0, 0, q + dt * qDot);
+ nextState.set(1, 0, qDot - dt * (b * qDot + m * g * l * Math.sin(q)) / I);
+
+ return nextState;
+ }
+
+ DMatrixRMaj getMeasurement(DMatrixRMaj state)
+ {
+ DMatrixRMaj measurement = new DMatrixRMaj(ExamplePendulumExtendedKalmanFilter.measurementSize, 1);
+ double q = state.get(0, 0);
+ double qDot = state.get(1, 0);
+
+ // Measurement is the total energy in the system, which will be dissipated with nonzero damping.
+ measurement.set(0, 0, 0.5 * I * qDot * qDot + m * g * l * (1 - Math.cos(q)));
+
+ return measurement;
+ }
+}
diff --git a/ihmc-parameter-estimation/src/test/java/us/ihmc/parameterEstimation/examples/ConstantVelocity2DExtendedKalmanFilterDemo.java b/ihmc-parameter-estimation/src/test/java/us/ihmc/parameterEstimation/examples/ConstantVelocity2DExtendedKalmanFilterDemo.java
index d7d40f8317bd..cd199e0b5f28 100644
--- a/ihmc-parameter-estimation/src/test/java/us/ihmc/parameterEstimation/examples/ConstantVelocity2DExtendedKalmanFilterDemo.java
+++ b/ihmc-parameter-estimation/src/test/java/us/ihmc/parameterEstimation/examples/ConstantVelocity2DExtendedKalmanFilterDemo.java
@@ -5,7 +5,6 @@
import org.knowm.xchart.QuickChart;
import org.knowm.xchart.SwingWrapper;
import org.knowm.xchart.XYChart;
-import us.ihmc.parameterEstimation.ExtendedKalmanFilter;
import us.ihmc.parameterEstimation.ExtendedKalmanFilterTestTools.LinearSystem;
import javax.swing.*;
@@ -22,77 +21,6 @@
*/
public class ConstantVelocity2DExtendedKalmanFilterDemo
{
- public static class ConstantVelocity2DKalmanFilter extends ExtendedKalmanFilter
- {
- private static final int stateSize = 4;
- private static final int measurementSize = 2;
-
- // Start at (x,y) = (0,0) with no velocity
- private static final DMatrixRMaj x0 = new DMatrixRMaj(new double[] {0.0, 0.0, 0.0, 0.0});
-
- // Position is barely affected by noise, velocity is affected by noise
- private static final DMatrixRMaj Q = new DMatrixRMaj(new double[][] {{1e-6, 0.0, 0.0, 0.0},
- {0.0, 1e-6, 0.0, 0.0},
- {0.0, 0.0, 1e-6, 0.0},
- {0.0, 0.0, 0.0, 1e-6}});
-
- // Both x and y position measurements are corrupted by noise
- private static final DMatrixRMaj R = new DMatrixRMaj(new double[][] {{1, 0.0}, {0.0, 1}});
-
- // Ignorant initial guess on P0, we assume we're more certain about positions than velocities
- private static final DMatrixRMaj P0 = new DMatrixRMaj(new double[][] {{0.1, 0.0, 0.0, 0.0},
- {0.0, 1.0, 0.0, 0.0},
- {0.0, 0.0, 0.1, 0.0},
- {0.0, 0.0, 0.0, 1.0}});
-
- private static final double dt = 0.01;
-
- // Constant velocity model of a 2D planar system
- private static final DMatrixRMaj A = new DMatrixRMaj(new double[][] {{1.0, dt, 0.0, 0.0},
- {0.0, 1.0, 0.0, 0.0},
- {0.0, 0.0, 1.0, dt},
- {0.0, 0.0, 0.0, 1.0}});
-
- // We only measure positions, not velocities
- private static final DMatrixRMaj C = new DMatrixRMaj(new double[][] {{1.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 1.0, 0.0}});
-
- public ConstantVelocity2DKalmanFilter()
- {
- super(x0, P0, Q, R);
- }
-
- // In the case of a linear process model, the linearization is just the A matrix of the process model
- @Override
- public DMatrixRMaj linearizeProcessModel(DMatrixRMaj previousState)
- {
- return A;
- }
-
- // In the case of a linear measurement model, the linearization is just the C matrix of the measurement model
- @Override
- public DMatrixRMaj linearizeMeasurementModel(DMatrixRMaj predictedState)
- {
- return C;
- }
-
- // y = Ax
- @Override
- public DMatrixRMaj processModel(DMatrixRMaj state)
- {
- DMatrixRMaj nextState = new DMatrixRMaj(stateSize, 1);
- CommonOps_DDRM.mult(A, state, nextState);
- return nextState;
- }
-
- // y = Cx
- @Override
- public DMatrixRMaj measurementModel(DMatrixRMaj state)
- {
- DMatrixRMaj measurement = new DMatrixRMaj(measurementSize, 1);
- CommonOps_DDRM.mult(C, state, measurement);
- return measurement;
- }
- }
private static final int ITERATIONS = 1000;
@@ -102,11 +30,11 @@ public static void main(String[] args)
// We'll set things up such that the real system is always less noisy than the filter. Said the other way, our filter design
// will assume that the system is more noisy than it actually is -- a good thing.
- DMatrixRMaj Q = new DMatrixRMaj(ConstantVelocity2DKalmanFilter.Q);
+ DMatrixRMaj Q = new DMatrixRMaj(ExampleConstantVelocity2DKalmanFilter.Q);
CommonOps_DDRM.scale(0.1, Q);
// We'll set things up such that the real system is always less noisy than the filter. Said the other way, our filter design
// will assume that the system is more noisy than it actually is -- a good thing.
- DMatrixRMaj R = new DMatrixRMaj(ConstantVelocity2DKalmanFilter.R);
+ DMatrixRMaj R = new DMatrixRMaj(ExampleConstantVelocity2DKalmanFilter.R);
CommonOps_DDRM.scale(0.01, R);
// In general, initial state of system is different from that of filter.
@@ -116,14 +44,14 @@ public static void main(String[] args)
// LinearSystem system = new LinearSystem(initialSystemState,
// ConstantVelocity2DKalmanFilter.A,
// ConstantVelocity2DKalmanFilter.C);
- LinearSystem system = new LinearSystem(initialSystemState, ConstantVelocity2DKalmanFilter.A, Q, ConstantVelocity2DKalmanFilter.C, R, random);
+ LinearSystem system = new LinearSystem(initialSystemState, ExampleConstantVelocity2DKalmanFilter.A, Q, ExampleConstantVelocity2DKalmanFilter.C, R, random);
- DMatrixRMaj state = new DMatrixRMaj(ConstantVelocity2DKalmanFilter.stateSize, 1);
- DMatrixRMaj measurement = new DMatrixRMaj(ConstantVelocity2DKalmanFilter.measurementSize, 1);
+ DMatrixRMaj state = new DMatrixRMaj(ExampleConstantVelocity2DKalmanFilter.stateSize, 1);
+ DMatrixRMaj measurement = new DMatrixRMaj(ExampleConstantVelocity2DKalmanFilter.measurementSize, 1);
// Filter
- ConstantVelocity2DKalmanFilter kalmanFilter = new ConstantVelocity2DKalmanFilter();
- DMatrixRMaj estimatedState = new DMatrixRMaj(ConstantVelocity2DKalmanFilter.stateSize, 1);
+ ExampleConstantVelocity2DKalmanFilter kalmanFilter = new ExampleConstantVelocity2DKalmanFilter();
+ DMatrixRMaj estimatedState = new DMatrixRMaj(ExampleConstantVelocity2DKalmanFilter.stateSize, 1);
// Arrays for recording data that we'll plot
double[] timestamps = new double[ITERATIONS];
@@ -139,7 +67,7 @@ public static void main(String[] args)
estimatedState.set(kalmanFilter.calculateEstimate(measurement));
- timestamps[i] = i * ConstantVelocity2DKalmanFilter.dt;
+ timestamps[i] = i * ExampleConstantVelocity2DKalmanFilter.dt;
trueStates.add(new DMatrixRMaj(state));
measurements.add(new DMatrixRMaj(measurement));
estimatedStates.add(new DMatrixRMaj(estimatedState));
diff --git a/ihmc-parameter-estimation/src/test/java/us/ihmc/parameterEstimation/examples/PendulumExtendedKalmanFilterDemo.java b/ihmc-parameter-estimation/src/test/java/us/ihmc/parameterEstimation/examples/PendulumExtendedKalmanFilterDemo.java
index 56ec3bc0537a..4e66031d5ca9 100644
--- a/ihmc-parameter-estimation/src/test/java/us/ihmc/parameterEstimation/examples/PendulumExtendedKalmanFilterDemo.java
+++ b/ihmc-parameter-estimation/src/test/java/us/ihmc/parameterEstimation/examples/PendulumExtendedKalmanFilterDemo.java
@@ -5,13 +5,13 @@
import org.knowm.xchart.QuickChart;
import org.knowm.xchart.SwingWrapper;
import org.knowm.xchart.XYChart;
-import us.ihmc.parameterEstimation.ExtendedKalmanFilter;
import us.ihmc.parameterEstimation.ExtendedKalmanFilterTestTools.NonlinearSystem;
import javax.swing.*;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Random;
+import java.util.function.Function;
/**
* Demo of an EKF being used with a nonlinear process model and measurement model.
@@ -30,139 +30,65 @@ public class PendulumExtendedKalmanFilterDemo
private static final double dt = 0.01;
- public static class PendulumExtendedKalmanFilter extends ExtendedKalmanFilter
- {
- private static final int stateSize = 2;
- private static final int measurementSize = 1;
-
- // Picking an arbitrary nonzero initial condition since 0 is a singularity for the measurement
- private static final DMatrixRMaj x0 = new DMatrixRMaj(new double[] {0.1, 0.1});
-
- // Process noise
- private static final DMatrixRMaj Q = new DMatrixRMaj(new double[][] {{1e-6, 0.0}, {0.0, 1e-6}});
-
- // Measurement noise
- private static final DMatrixRMaj R = new DMatrixRMaj(new double[][] {{1e-3}});
-
- // Ignorant initial guess on P0, we assume we're more certain about positions than velocities
- private static final DMatrixRMaj P0 = new DMatrixRMaj(new double[][] {{0.1, 0.0}, {0.0, 1.0}});
-
- // Linearized process model dynamics ( Jacobian of f(x) where x[k+1] = f(x[k]) )
- private static final DMatrixRMaj F = new DMatrixRMaj(stateSize, stateSize);
-
- // Linearized measurement model dynamics (Jacobian of h(x) where y = h(x))
- private static final DMatrixRMaj H = new DMatrixRMaj(measurementSize, stateSize);
-
- public PendulumExtendedKalmanFilter()
- {
- super(x0, P0, Q, R);
- }
-
- // Linearize f(x) to obtain it's Jacobian matrix, F(x)
- @Override
- public DMatrixRMaj linearizeProcessModel(DMatrixRMaj previousState)
- {
- double q = previousState.get(0, 0);
-
- F.set(0, 0, 1);
- F.set(0, 1, dt);
- F.set(1, 0, -(dt * m * g * l * Math.cos(q)) / I);
- F.set(1, 1, 1 - dt * b / I);
-
- return F;
- }
-
- // Linearize h(x) to obtain it's Jacobian matrix, H(x)
- @Override
- public DMatrixRMaj linearizeMeasurementModel(DMatrixRMaj predictedState)
- {
- double q = predictedState.get(0, 0);
- double qDot = predictedState.get(1, 0);
-
- H.set(0, 0, m * g * l * Math.sin(q));
- H.set(0, 1, I * qDot);
-
- return H;
- }
-
- // x[k+1] = f(x[k])
- @Override
- public DMatrixRMaj processModel(DMatrixRMaj state)
- {
- return getNextState(state);
- }
-
- // y = h(x)
- @Override
- public DMatrixRMaj measurementModel(DMatrixRMaj state)
- {
- return getMeasurement(state);
- }
- }
-
private static class PendulumSystem extends NonlinearSystem
{
- public PendulumSystem(DMatrixRMaj x0, DMatrixRMaj F, DMatrixRMaj H)
+ private final Function systemDynamics;
+ private final Function measurementDynamics;
+
+ public PendulumSystem(DMatrixRMaj x0,
+ DMatrixRMaj F,
+ DMatrixRMaj H,
+ Function systemDynamics,
+ Function measurementDynamics)
{
super(x0, F, H);
+
+ this.systemDynamics = systemDynamics;
+ this.measurementDynamics = measurementDynamics;
}
- public PendulumSystem(DMatrixRMaj x0, DMatrixRMaj F, DMatrixRMaj Q, DMatrixRMaj H, DMatrixRMaj R, Random random)
+ public PendulumSystem(DMatrixRMaj x0, DMatrixRMaj F, DMatrixRMaj Q, DMatrixRMaj H, DMatrixRMaj R, Random random,
+ Function systemDynamics,
+ Function measurementDynamics)
{
super(x0, F, Q, H, R, random);
+
+ this.systemDynamics = systemDynamics;
+ this.measurementDynamics = measurementDynamics;
+
}
@Override
public DMatrixRMaj calculateSystemDynamics(DMatrixRMaj state)
{
- return getNextState(state);
+ return systemDynamics.apply(state);
}
@Override
public DMatrixRMaj calculateMeasurementDynamics(DMatrixRMaj state)
{
- return getMeasurement(state);
+ return measurementDynamics.apply(state);
}
}
- private static DMatrixRMaj getNextState(DMatrixRMaj state)
- {
- DMatrixRMaj nextState = new DMatrixRMaj(state.numRows, 1);
- double q = state.get(0, 0);
- double qDot = state.get(1, 0);
-
- // Discretized with Forward Euler
- nextState.set(0, 0, q + dt * qDot);
- nextState.set(1, 0, qDot - dt * (b * qDot + m * g * l * Math.sin(q)) / I);
-
- return nextState;
- }
-
- private static DMatrixRMaj getMeasurement(DMatrixRMaj state)
- {
- DMatrixRMaj measurement = new DMatrixRMaj(PendulumExtendedKalmanFilter.measurementSize, 1);
- double q = state.get(0, 0);
- double qDot = state.get(1, 0);
-
- // Measurement is the total energy in the system, which will be dissipated with nonzero damping.
- measurement.set(0, 0, 0.5 * I * qDot * qDot + m * g * l * (1 - Math.cos(q)));
-
- return measurement;
- }
-
private static final int ITERATIONS = 500;
public static void main(String[] args)
{
Random random = new Random(45);
+ // Filter
+ ExamplePendulumExtendedKalmanFilter kalmanFilter = new ExamplePendulumExtendedKalmanFilter();
+ kalmanFilter.setModelProperties(m, I, l, b, g, dt);
+ DMatrixRMaj estimatedState = new DMatrixRMaj(ExamplePendulumExtendedKalmanFilter.stateSize, 1);
+
// We'll set things up such that the real system is always less noisy than the filter. Said the other way, our filter design
// will assume that the system is more noisy than it actually is -- a good thing.
- DMatrixRMaj Q = new DMatrixRMaj(PendulumExtendedKalmanFilter.Q);
+ DMatrixRMaj Q = new DMatrixRMaj(kalmanFilter.getQ());
CommonOps_DDRM.scale(0.1, Q);
// We'll set things up such that the real system is always less noisy than the filter. Said the other way, our filter design
// will assume that the system is more noisy than it actually is -- a good thing.
- DMatrixRMaj R = new DMatrixRMaj(PendulumExtendedKalmanFilter.R);
+ DMatrixRMaj R = new DMatrixRMaj(kalmanFilter.getR());
CommonOps_DDRM.scale(0.01, R);
// In general, initial state of system is different from that of filter.
@@ -172,14 +98,11 @@ public static void main(String[] args)
// NonlinearSystem system = new ConstantVelocity2DNonlinearMeasurementSystem(initialSystemState,
// PendulumExtendedKalmanFilter.F,
// PendulumExtendedKalmanFilter.H);
- NonlinearSystem system = new PendulumSystem(initialSystemState, PendulumExtendedKalmanFilter.F, Q, PendulumExtendedKalmanFilter.H, R, random);
-
- DMatrixRMaj state = new DMatrixRMaj(PendulumExtendedKalmanFilter.stateSize, 1);
- DMatrixRMaj measurement = new DMatrixRMaj(PendulumExtendedKalmanFilter.measurementSize, 1);
+ NonlinearSystem system = new PendulumSystem(initialSystemState, kalmanFilter.getF(), Q, kalmanFilter.getH(), R, random,
+ kalmanFilter::getNextState, kalmanFilter::getMeasurement);
- // Filter
- PendulumExtendedKalmanFilter kalmanFilter = new PendulumExtendedKalmanFilter();
- DMatrixRMaj estimatedState = new DMatrixRMaj(PendulumExtendedKalmanFilter.stateSize, 1);
+ DMatrixRMaj state = new DMatrixRMaj(ExamplePendulumExtendedKalmanFilter.stateSize, 1);
+ DMatrixRMaj measurement = new DMatrixRMaj(ExamplePendulumExtendedKalmanFilter.measurementSize, 1);
// Arrays for recording data that we'll plot
double[] timestamps = new double[ITERATIONS];
diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/sensors/IMUDefinition.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/sensors/IMUDefinition.java
index df1b2287416d..2f87bee20642 100644
--- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/sensors/IMUDefinition.java
+++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/sensors/IMUDefinition.java
@@ -4,6 +4,7 @@
import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.transform.interfaces.RigidBodyTransformReadOnly;
+import us.ihmc.mecano.frames.MovingReferenceFrame;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
public class IMUDefinition
@@ -11,7 +12,7 @@ public class IMUDefinition
private final String name;
private final RigidBodyBasics rigidBody;
private final RigidBodyTransform transformFromIMUToJoint;
- private final ReferenceFrame imuFrame;
+ private final MovingReferenceFrame imuFrame;
public IMUDefinition(String name, RigidBodyBasics rigidBody, RigidBodyTransformReadOnly transformFromIMUToJoint)
{
@@ -19,8 +20,8 @@ public IMUDefinition(String name, RigidBodyBasics rigidBody, RigidBodyTransformR
this.rigidBody = rigidBody;
this.transformFromIMUToJoint = new RigidBodyTransform(transformFromIMUToJoint);
- ReferenceFrame frameAfterJoint = rigidBody.getParentJoint().getFrameAfterJoint();
- imuFrame = ReferenceFrameTools.constructFrameWithUnchangingTransformToParent(name, frameAfterJoint, transformFromIMUToJoint);
+ MovingReferenceFrame frameAfterJoint = rigidBody.getParentJoint().getFrameAfterJoint();
+ imuFrame = MovingReferenceFrame.constructFrameFixedInParent(name, frameAfterJoint, transformFromIMUToJoint);
}
public String getName()
@@ -38,7 +39,7 @@ public RigidBodyTransformReadOnly getTransformFromIMUToJoint()
return transformFromIMUToJoint;
}
- public ReferenceFrame getIMUFrame()
+ public MovingReferenceFrame getIMUFrame()
{
return imuFrame;
}
diff --git a/ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/imu/IMUSensor.java b/ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/imu/IMUSensor.java
index d69575551967..5fffa5c85b42 100644
--- a/ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/imu/IMUSensor.java
+++ b/ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/imu/IMUSensor.java
@@ -10,6 +10,7 @@
import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly;
import us.ihmc.euclid.tuple4D.Quaternion;
import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly;
+import us.ihmc.mecano.frames.MovingReferenceFrame;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.robotics.sensors.IMUDefinition;
import us.ihmc.sensorProcessing.simulatedSensors.SensorNoiseParameters;
@@ -19,7 +20,7 @@ public class IMUSensor implements IMUSensorReadOnly
{
private final String sensorName;
- private final ReferenceFrame measurementFrame;
+ private final MovingReferenceFrame measurementFrame;
private final RigidBodyBasics measurementLink;
private final Quaternion orientationMeasurement = new Quaternion();
@@ -68,7 +69,7 @@ public String getSensorName()
}
@Override
- public ReferenceFrame getMeasurementFrame()
+ public MovingReferenceFrame getMeasurementFrame()
{
return measurementFrame;
}
diff --git a/ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/stateEstimation/IMUSensorReadOnly.java b/ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/stateEstimation/IMUSensorReadOnly.java
index c1e048fde073..06056ed439ac 100644
--- a/ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/stateEstimation/IMUSensorReadOnly.java
+++ b/ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/stateEstimation/IMUSensorReadOnly.java
@@ -7,13 +7,14 @@
import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics;
import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly;
import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly;
+import us.ihmc.mecano.frames.MovingReferenceFrame;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
public interface IMUSensorReadOnly
{
public abstract String getSensorName();
- public abstract ReferenceFrame getMeasurementFrame();
+ public abstract MovingReferenceFrame getMeasurementFrame();
public abstract RigidBodyBasics getMeasurementLink();
diff --git a/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/DRCKinematicsBasedStateEstimator.java b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/DRCKinematicsBasedStateEstimator.java
index cc9fb8862086..1f0ed3240994 100644
--- a/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/DRCKinematicsBasedStateEstimator.java
+++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/DRCKinematicsBasedStateEstimator.java
@@ -39,6 +39,7 @@
import us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.centerOfMassEstimator.MomentumStateUpdater;
import us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.centerOfMassEstimator.SimpleMomentumStateUpdater;
import us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.centerOfMassEstimator.WrenchBasedMomentumStateUpdater;
+import us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF.OdometryKalmanFilter;
import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePose3D;
import us.ihmc.yoVariables.parameters.BooleanParameter;
import us.ihmc.yoVariables.providers.BooleanProvider;
@@ -65,6 +66,7 @@ public class DRCKinematicsBasedStateEstimator implements StateEstimatorControlle
private final MomentumStateUpdater momentumStateUpdater;
private final IMUBiasStateEstimator imuBiasStateEstimator;
private final IMUYawDriftEstimator imuYawDriftEstimator;
+ private final OdometryKalmanFilter odometry;
private final PelvisPoseHistoryCorrectionInterface pelvisPoseHistoryCorrection;
@@ -107,6 +109,7 @@ public DRCKinematicsBasedStateEstimator(FullInverseDynamicsStructure inverseDyna
rootJoint = inverseDynamicsStructure.getRootJoint();
+
usePelvisCorrector = new YoBoolean("useExternalPelvisCorrector", registry);
usePelvisCorrector.set(true);
@@ -144,6 +147,7 @@ public DRCKinematicsBasedStateEstimator(FullInverseDynamicsStructure inverseDyna
List imusToUse = new ArrayList<>(imuProcessedOutputs);
+
BooleanProvider cancelGravityFromAccelerationMeasurement = new BooleanParameter("cancelGravityFromAccelerationMeasurement",
registry,
stateEstimatorParameters.cancelGravityFromAccelerationMeasurement());
@@ -162,6 +166,10 @@ public DRCKinematicsBasedStateEstimator(FullInverseDynamicsStructure inverseDyna
stateEstimatorParameters,
registry);
+
+ List feetIMUs = imusToUse.stream().filter(sensor -> feet.containsKey(sensor.getMeasurementLink())).toList();
+ odometry = new OdometryKalmanFilter(imusToUse.get(0), feetIMUs, imuBiasStateEstimator, cancelGravityFromAccelerationMeasurement, estimatorDT, gravitationalAcceleration, registry);
+
jointStateUpdater = new JointStateUpdater(inverseDynamicsStructure, sensorOutputMap, stateEstimatorParameters, registry);
if (forceSensorDataHolderToUpdate != null)
@@ -336,6 +344,8 @@ public void doControl()
for (int i = 0; i < footSwitchList.size(); i++)
footSwitchList.get(i).update();
+
+
switch (operatingMode.getEnumValue())
{
case FROZEN:
@@ -347,6 +357,9 @@ public void doControl()
break;
}
+ // TODO should this also consume some of the foot switch data?
+ odometry.compute();
+
if (momentumStateUpdater != null)
momentumStateUpdater.update();
diff --git a/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/PelvisKinematicsBasedLinearStateCalculator.java b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/PelvisKinematicsBasedLinearStateCalculator.java
index 8641f12b2153..7b023c24ec7a 100644
--- a/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/PelvisKinematicsBasedLinearStateCalculator.java
+++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/PelvisKinematicsBasedLinearStateCalculator.java
@@ -4,45 +4,22 @@
import java.util.List;
import java.util.Map;
-import us.ihmc.euclid.referenceFrame.FrameConvexPolygon2D;
-import us.ihmc.euclid.referenceFrame.FrameLineSegment2D;
-import us.ihmc.euclid.referenceFrame.FramePoint2D;
import us.ihmc.euclid.referenceFrame.FramePoint3D;
-import us.ihmc.euclid.referenceFrame.FrameVector3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
-import us.ihmc.euclid.referenceFrame.interfaces.FixedFramePoint3DBasics;
-import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameVector3DBasics;
-import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DBasics;
import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly;
import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly;
-import us.ihmc.euclid.referenceFrame.interfaces.FrameVertex2DSupplier;
-import us.ihmc.graphicsDescription.appearance.YoAppearance;
-import us.ihmc.graphicsDescription.yoGraphics.YoGraphicPosition;
import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry;
-import us.ihmc.graphicsDescription.yoGraphics.plotting.YoArtifactPosition;
import us.ihmc.humanoidRobotics.model.CenterOfPressureDataHolder;
-import us.ihmc.log.LogTools;
-import us.ihmc.mecano.multiBodySystem.interfaces.FloatingJointBasics;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
-import us.ihmc.mecano.spatial.Twist;
-import us.ihmc.mecano.spatial.interfaces.TwistReadOnly;
-import us.ihmc.robotics.SCS2YoGraphicHolder;
import us.ihmc.robotics.contactable.ContactablePlaneBody;
-import us.ihmc.yoVariables.euclid.filters.AlphaFilteredYoFramePoint2D;
-import us.ihmc.yoVariables.euclid.filters.AlphaFilteredYoFrameVector3D;
+import us.ihmc.sensorProcessing.stateEstimation.IMUSensorReadOnly;
import us.ihmc.yoVariables.euclid.filters.BacklashCompensatingVelocityYoFrameVector3D;
import us.ihmc.robotics.sensors.FootSwitchInterface;
-import us.ihmc.scs2.definition.visual.ColorDefinitions;
-import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinition;
-import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinitionFactory;
-import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinitionFactory.DefaultPoint2DGraphic;
-import us.ihmc.scs2.definition.yoGraphic.YoGraphicGroupDefinition;
-import us.ihmc.scs2.definition.yoGraphic.YoGraphicPoint2DDefinition;
import us.ihmc.sensorProcessing.stateEstimation.StateEstimatorParameters;
import us.ihmc.sensorProcessing.stateEstimation.evaluation.FullInverseDynamicsStructure;
-import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint2D;
import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D;
import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D;
+import us.ihmc.yoVariables.filters.AlphaFilterTools;
import us.ihmc.yoVariables.filters.AlphaFilteredYoVariable;
import us.ihmc.yoVariables.parameters.BooleanParameter;
import us.ihmc.yoVariables.parameters.DoubleParameter;
@@ -58,87 +35,52 @@
*
* @author Sylvain
*/
-public class PelvisKinematicsBasedLinearStateCalculator implements SCS2YoGraphicHolder
+public class PelvisKinematicsBasedLinearStateCalculator
{
- private static final boolean VISUALIZE = true;
-
private final YoRegistry registry = new YoRegistry(getClass().getSimpleName());
- private final FloatingJointBasics rootJoint;
- private final RigidBodyBasics[] feetRigidBodies;
- private final SingleFootEstimator[] footEstimators;
- private final Map footEstimatorMap = new HashMap<>();
+ private final Map footEstimatorMap;
private final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
private final YoFramePoint3D rootJointPosition = new YoFramePoint3D("estimatedRootJointPositionKinematics", worldFrame, registry);
private final YoFrameVector3D rootJointLinearVelocity = new YoFrameVector3D("estimatedRootJointLinearVelocityKinematics", worldFrame, registry);
+ private final YoFrameVector3D averagedTrustedFootVelocity = new YoFrameVector3D("averagedTrustedFootVelocity", worldFrame, registry);
private final DoubleProvider alphaRootJointLinearVelocity;
- /** Debug variable */
- private final YoDouble alphaLinearVelocityDebug = new YoDouble("alphaRootJointLinearVelocityBacklashKinematics", registry);
- /** Debug variable */
- private final YoDouble slopTimeLinearVelocityDebug = new YoDouble("slopTimeRootJointLinearVelocityBacklashKinematics", registry);
/** Debug variable */
private final BacklashCompensatingVelocityYoFrameVector3D rootJointLinearVelocityFDDebug;
- private final DoubleProvider footToRootJointPositionBreakFrequency;
private final BooleanProvider correctTrustedFeetPositions;
- private final DoubleProvider copFilterBreakFrequency;
-
private final YoBoolean kinematicsIsUpToDate = new YoBoolean("kinematicsIsUpToDate", registry);
- private final BooleanProvider useControllerDesiredCoP;
private final BooleanProvider trustCoPAsNonSlippingContactPoint;
private final BooleanParameter assumeTrustedFootAtZeroHeight = new BooleanParameter("assumeTrustedFootAtZeroHeight", registry, false);
+ private final BooleanProvider useControllerDesiredCoP;
- public PelvisKinematicsBasedLinearStateCalculator(FullInverseDynamicsStructure inverseDynamicsStructure,
- Map feetContactablePlaneBodies,
- Map footSwitches,
- CenterOfPressureDataHolder centerOfPressureDataHolderFromController,
+ public PelvisKinematicsBasedLinearStateCalculator(Map footEstimatorMap,
double estimatorDT,
StateEstimatorParameters stateEstimatorParameters,
- YoGraphicsListRegistry yoGraphicsListRegistry,
YoRegistry parentRegistry)
{
- rootJoint = inverseDynamicsStructure.getRootJoint();
- feetRigidBodies = feetContactablePlaneBodies.keySet().toArray(new RigidBodyBasics[0]);
-
- footToRootJointPositionBreakFrequency = new DoubleParameter("FootToRootJointPositionBreakFrequency",
- registry,
- stateEstimatorParameters.getKinematicsPelvisPositionFilterFreqInHertz());
+ this.footEstimatorMap = footEstimatorMap;
alphaRootJointLinearVelocity = new DoubleParameter("alphaRootJointLinearVelocityKinematics",
registry,
stateEstimatorParameters.getPelvisLinearVelocityAlphaNewTwist());
trustCoPAsNonSlippingContactPoint = new BooleanParameter("trustCoPAsNonSlippingContactPoint",
registry,
stateEstimatorParameters.trustCoPAsNonSlippingContactPoint());
- useControllerDesiredCoP = new BooleanParameter("useControllerDesiredCoP", registry, stateEstimatorParameters.useControllerDesiredCenterOfPressure());
- copFilterBreakFrequency = new DoubleParameter("CopFilterBreakFrequency", registry, stateEstimatorParameters.getCoPFilterFreqInHertz());
- correctTrustedFeetPositions = new BooleanParameter("correctTrustedFeetPositions", registry, stateEstimatorParameters.correctTrustedFeetPositions());
- footEstimators = new SingleFootEstimator[feetRigidBodies.length];
- for (int i = 0; i < feetRigidBodies.length; i++)
- {
- RigidBodyBasics footRigidBody = feetRigidBodies[i];
- ContactablePlaneBody contactableFoot = feetContactablePlaneBodies.get(footRigidBody);
- FootSwitchInterface footSwitch = footSwitches.get(footRigidBody);
- footEstimators[i] = new SingleFootEstimator(rootJoint,
- contactableFoot,
- footSwitch,
- footToRootJointPositionBreakFrequency,
- copFilterBreakFrequency,
- centerOfPressureDataHolderFromController,
- estimatorDT,
- registry);
- footEstimatorMap.put(footRigidBody, footEstimators[i]);
- }
+ correctTrustedFeetPositions = new BooleanParameter("correctTrustedFeetPositions", registry, stateEstimatorParameters.correctTrustedFeetPositions());
+ useControllerDesiredCoP = new BooleanParameter("useControllerDesiredCoP", registry, stateEstimatorParameters.useControllerDesiredCenterOfPressure());
/*
* These are for debug purposes, not need to clutter the state estimator parameters class with them.
*/
- alphaLinearVelocityDebug.set(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(16.0, estimatorDT));
+ YoDouble alphaLinearVelocityDebug = new YoDouble("alphaRootJointLinearVelocityBacklashKinematics", registry);
+ YoDouble slopTimeLinearVelocityDebug = new YoDouble("slopTimeRootJointLinearVelocityBacklashKinematics", registry);
+ alphaLinearVelocityDebug.set(AlphaFilterTools.computeAlphaGivenBreakFrequencyProperly(16.0, estimatorDT));
slopTimeLinearVelocityDebug.set(0.03);
rootJointLinearVelocityFDDebug = new BacklashCompensatingVelocityYoFrameVector3D("estimatedRootJointLinearVelocityBacklashKin",
"",
@@ -151,455 +93,125 @@ public PelvisKinematicsBasedLinearStateCalculator(FullInverseDynamicsStructure i
* -------------------------------------------------------------------------------------------------
*/
- if (VISUALIZE)
- {
- for (SingleFootEstimator footEstimator : footEstimators)
- {
- footEstimator.createVisualization(yoGraphicsListRegistry);
- }
- }
parentRegistry.addChild(registry);
}
/**
- * Estimates the foot positions corresponding to the given pelvisPosition
+ * Estimates the foot positions corresponding to the given rootJointPosition
*
- * @param pelvisPosition
+ * @param rootJointPosition
*/
- public void initialize(FramePoint3DReadOnly pelvisPosition)
+ public void initialize(FramePoint3DReadOnly rootJointPosition)
{
- for (SingleFootEstimator footEstimator : footEstimators)
- {
- footEstimator.initialize();
- }
- setPelvisLinearVelocityToZero();
+ rootJointLinearVelocityFDDebug.reset();
+ rootJointLinearVelocityFDDebug.setToZero();
+ rootJointLinearVelocity.setToZero();
- updateKinematics();
- setPelvisPosition(pelvisPosition);
+ this.rootJointPosition.set(rootJointPosition);
- for (SingleFootEstimator footEstimator : footEstimators)
- {
- footEstimator.updateUntrustedFootPosition(pelvisPosition);
- }
kinematicsIsUpToDate.set(false);
}
+
/**
* Updates the different kinematics related stuff that is used to estimate the pelvis state
*/
public void updateKinematics()
{
rootJointPosition.setToZero();
- updateKinematicsNewTwist();
-
- for (SingleFootEstimator footEstimator : footEstimators)
- footEstimator.updateKinematics();
-
kinematicsIsUpToDate.set(true);
}
- private final Twist tempRootBodyTwist = new Twist();
-
- private void updateKinematicsNewTwist()
- {
- tempRootBodyTwist.setIncludingFrame(rootJoint.getJointTwist());
- tempRootBodyTwist.getLinearPart().setMatchingFrame(rootJointLinearVelocity);
-
- for (SingleFootEstimator footEstimator : footEstimators)
- footEstimator.updateFootLinearVelocityInWorld(tempRootBodyTwist);
- }
-
/**
* Updates this module when no foot can be trusted.
*
- * @param pelvisPosition the current estimated pelvis position. This module does not compute
+ * @param rootJointPosition the current estimated pelvis position. This module does not compute
* an estimate.
- * @param pelvisLinearVelocity the current estimated pelvis linear velocity. This module does not
+ * @param rootJointLinearVelocity the current estimated pelvis linear velocity. This module does not
* compute an estimate. If {@code null}, a zero velocity is assumed.
*/
- public void updateNoTrustedFeet(FramePoint3DReadOnly pelvisPosition, FrameVector3DReadOnly pelvisLinearVelocity)
+ public void setRootJointState(FramePoint3DReadOnly rootJointPosition, FrameVector3DReadOnly rootJointLinearVelocity)
{
- for (SingleFootEstimator footEstimator : footEstimators)
- {
- footEstimator.updateUntrustedFootPosition(pelvisPosition);
- }
-
- rootJointPosition.set(pelvisPosition);
+ this.rootJointPosition.set(rootJointPosition);
- if (pelvisLinearVelocity != null)
- rootJointLinearVelocity.set(pelvisLinearVelocity);
+ if (rootJointLinearVelocity != null)
+ this.rootJointLinearVelocity.set(rootJointLinearVelocity);
else
- rootJointLinearVelocity.setToZero();
- }
-
- public void estimatePelvisLinearState(List trustedFeet, List unTrustedFeet, FramePoint3DReadOnly pelvisPosition)
- {
- if (!kinematicsIsUpToDate.getBooleanValue())
- throw new RuntimeException("Leg kinematics needs to be updated before trying to estimate the pelvis position/linear velocity.");
-
- if (assumeTrustedFootAtZeroHeight.getValue())
- {
- for (int i = 0; i < trustedFeet.size(); i++)
- {
- SingleFootEstimator footEstimator = footEstimatorMap.get(trustedFeet.get(i));
- if (trustCoPAsNonSlippingContactPoint.getValue())
- {
- footEstimator.updateCoPPosition(trustCoPAsNonSlippingContactPoint.getValue(), useControllerDesiredCoP.getValue());
- footEstimator.copPositionInWorld.setZ(0.0);
- footEstimator.correctFootPositionsUsingCoP(trustCoPAsNonSlippingContactPoint.getValue());
- }
- else
- {
- footEstimator.footPositionInWorld.setZ(0.0);
- }
- footEstimator.updatePelvisWithKinematics(trustedFeet.size(), alphaRootJointLinearVelocity.getValue(), rootJointPosition, rootJointLinearVelocity);
- }
- }
- else
- {
- for (int i = 0; i < trustedFeet.size(); i++)
- {
- SingleFootEstimator footEstimator = footEstimatorMap.get(trustedFeet.get(i));
- footEstimator.updateCoPPosition(trustCoPAsNonSlippingContactPoint.getValue(), useControllerDesiredCoP.getValue());
- footEstimator.correctFootPositionsUsingCoP(trustCoPAsNonSlippingContactPoint.getValue());
- footEstimator.updatePelvisWithKinematics(trustedFeet.size(), alphaRootJointLinearVelocity.getValue(), rootJointPosition, rootJointLinearVelocity);
- }
- }
-
- rootJointLinearVelocityFDDebug.update();
-
- if (correctTrustedFeetPositions.getValue())
- {
- for (int i = 0; i < trustedFeet.size(); i++)
- {
- SingleFootEstimator footEstimator = footEstimatorMap.get(trustedFeet.get(i));
- footEstimator.updateTrustedFootPosition(trustCoPAsNonSlippingContactPoint.getValue(), rootJointPosition);
- }
- }
-
- for (int i = 0; i < unTrustedFeet.size(); i++)
- {
- SingleFootEstimator footEstimator = footEstimatorMap.get(unTrustedFeet.get(i));
- footEstimator.updateUntrustedFootPosition(pelvisPosition);
- }
-
- kinematicsIsUpToDate.set(false);
- }
-
- public void setPelvisPosition(FramePoint3DReadOnly pelvisPosition)
- {
- rootJointPosition.set(pelvisPosition);
- }
-
- public void setPelvisLinearVelocity(FrameVector3DReadOnly pelvisLinearVelocity)
- {
- rootJointLinearVelocityFDDebug.reset();
- rootJointLinearVelocityFDDebug.set(pelvisLinearVelocity);
- rootJointLinearVelocity.set(pelvisLinearVelocity);
+ this.rootJointLinearVelocity.setToZero();
}
- public void setPelvisLinearVelocityToZero()
+ public boolean getKinematicsIsUpToDate()
{
- rootJointLinearVelocityFDDebug.reset();
- rootJointLinearVelocityFDDebug.setToZero();
- rootJointLinearVelocity.setToZero();
+ return kinematicsIsUpToDate.getBooleanValue();
}
- public FramePoint3DReadOnly getPelvisPosition()
+ public boolean getAssumeTrustedFootAtZeroHeight()
{
- return rootJointPosition;
+ return assumeTrustedFootAtZeroHeight.getValue();
}
- public FrameVector3DReadOnly getPelvisVelocity()
+ public boolean getTrustCoPAsNonSlippingContactPoint()
{
- return rootJointLinearVelocity;
+ return trustCoPAsNonSlippingContactPoint.getValue();
}
- public void getFootToRootJointPosition(FramePoint3D positionToPack, RigidBodyBasics foot)
+ public boolean getUseControllerDesiredCoP()
{
- positionToPack.setIncludingFrame(footEstimatorMap.get(foot).footToRootJointPosition);
+ return useControllerDesiredCoP.getValue();
}
- public FrameVector3DReadOnly getFootVelocityInWorld(RigidBodyBasics foot)
+ public boolean getCorrectTrustedFeetPositions()
{
- return footEstimatorMap.get(foot).footVelocityInWorld;
+ return correctTrustedFeetPositions.getValue();
}
- private static class SingleFootEstimator implements SCS2YoGraphicHolder
+ public void estimatePelvisLinearState(List trustedFeet, FramePoint3DReadOnly pelvisPosition, FrameVector3DReadOnly pelvisLinearVelocity)
{
- private final RigidBodyBasics foot;
-
- private final ReferenceFrame rootJointFrame;
- private final ReferenceFrame soleFrame;
- private final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
-
- private final YoFrameVector3D footVelocityInWorld;
- private final AlphaFilteredYoFrameVector3D footToRootJointPosition;
- private final YoFramePoint3D footPositionInWorld;
- /** Debug variable */
- private final YoFramePoint3D rootJointPositionPerFoot;
- private final YoFramePoint3D copPositionInWorld;
- private final AlphaFilteredYoFramePoint2D copFilteredInFootFrame;
- private final YoFramePoint2D copRawInFootFrame;
- private final FrameConvexPolygon2D footPolygon;
- private final FrameLineSegment2D footCenterCoPLineSegment;
- private final FootSwitchInterface footSwitch;
- private final CenterOfPressureDataHolder centerOfPressureDataHolderFromController;
-
- private final FramePoint2DBasics[] intersectionPoints = new FramePoint2DBasics[] {new FramePoint2D(), new FramePoint2D()};
-
- public SingleFootEstimator(FloatingJointBasics rootJoint,
- ContactablePlaneBody contactableFoot,
- FootSwitchInterface footSwitch,
- DoubleProvider footToRootJointPositionBreakFrequency,
- DoubleProvider copFilterBreakFrequency,
- CenterOfPressureDataHolder centerOfPressureDataHolderFromController,
- double estimatorDT,
- YoRegistry registry)
- {
- this.rootJointFrame = rootJoint.getFrameAfterJoint();
- this.footSwitch = footSwitch;
- this.centerOfPressureDataHolderFromController = centerOfPressureDataHolderFromController;
- foot = contactableFoot.getRigidBody();
- soleFrame = contactableFoot.getContactFrame();
-
- String namePrefix = foot.getName();
-
- DoubleProvider alphaFoot = () -> AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(footToRootJointPositionBreakFrequency.getValue(),
- estimatorDT);
- footToRootJointPosition = new AlphaFilteredYoFrameVector3D(namePrefix + "FootToRootJointPosition", "", registry, alphaFoot, worldFrame);
- rootJointPositionPerFoot = new YoFramePoint3D(namePrefix + "BasedRootJointPosition", worldFrame, registry);
- footPositionInWorld = new YoFramePoint3D(namePrefix + "FootPositionInWorld", worldFrame, registry);
- footPolygon = new FrameConvexPolygon2D(FrameVertex2DSupplier.asFrameVertex2DSupplier(contactableFoot.getContactPoints2D()));
- footCenterCoPLineSegment = new FrameLineSegment2D(soleFrame);
- copRawInFootFrame = new YoFramePoint2D(namePrefix + "CoPRawInFootFrame", soleFrame, registry);
-
- DoubleProvider alphaCop = () -> AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(copFilterBreakFrequency.getValue(), estimatorDT);
- copFilteredInFootFrame = new AlphaFilteredYoFramePoint2D(namePrefix + "CoPFilteredInFootFrame", "", registry, alphaCop, copRawInFootFrame);
- copFilteredInFootFrame.update(0.0, 0.0);
- copPositionInWorld = new YoFramePoint3D(namePrefix + "CoPPositionsInWorld", worldFrame, registry);
- footVelocityInWorld = new YoFrameVector3D(namePrefix + "VelocityInWorld", worldFrame, registry);
- }
-
- public void createVisualization(YoGraphicsListRegistry yoGraphicsListRegistry)
- {
- if (yoGraphicsListRegistry == null)
- return;
-
- String sidePrefix = foot.getName();
- YoGraphicPosition copInWorld = new YoGraphicPosition(sidePrefix + "StateEstimatorCoP", copPositionInWorld, 0.005, YoAppearance.DeepPink());
- YoArtifactPosition artifact = copInWorld.createArtifact();
- artifact.setVisible(false);
- yoGraphicsListRegistry.registerArtifact("StateEstimator", artifact);
- }
-
- @Override
- public YoGraphicDefinition getSCS2YoGraphics()
- {
- YoGraphicPoint2DDefinition copVisual = YoGraphicDefinitionFactory.newYoGraphicPoint2D(foot.getName() + "StateEstimatorCoP",
- copPositionInWorld,
- 0.01,
- ColorDefinitions.DeepPink(),
- DefaultPoint2DGraphic.CIRCLE);
- return copVisual;
- }
-
- public void initialize()
- {
- footToRootJointPosition.reset();
- copFilteredInFootFrame.reset();
- copFilteredInFootFrame.update(0.0, 0.0);
- footVelocityInWorld.setToZero();
- }
-
- private final FramePoint3D tempFramePoint = new FramePoint3D();
-
- /**
- * Estimates the pelvis position and linear velocity using the leg kinematics
- *
- * @param trustedFoot is the foot used to estimates the pelvis state
- * @param numberOfTrustedSides is only one or both legs used to estimate the pelvis state
- */
- private void updatePelvisWithKinematics(int numberOfTrustedFeet,
- double alphaVelocityUpdate,
- FixedFramePoint3DBasics rootJointPosition,
- FixedFrameVector3DBasics rootJointLinearVelocity)
- {
- double scaleFactor = 1.0 / numberOfTrustedFeet;
-
- rootJointPositionPerFoot.add(footPositionInWorld, footToRootJointPosition);
- rootJointPosition.scaleAdd(scaleFactor, rootJointPositionPerFoot, rootJointPosition);
-
- rootJointLinearVelocity.scaleAdd(-scaleFactor * alphaVelocityUpdate, footVelocityInWorld, rootJointLinearVelocity);
- }
+ if (!kinematicsIsUpToDate.getBooleanValue())
+ throw new RuntimeException("Leg kinematics needs to be updated before trying to estimate the pelvis position/linear velocity.");
- /**
- * updates the position of a swinging foot
- *
- * @param swingingFoot a foot in swing
- * @param pelvisPosition the current pelvis position
- */
- private void updateUntrustedFootPosition(FramePoint3DReadOnly pelvisPosition)
+ if (trustedFeet.isEmpty())
{
- footPositionInWorld.sub(pelvisPosition, footToRootJointPosition);
-
- copPositionInWorld.set(footPositionInWorld);
-
- copRawInFootFrame.setToZero();
- copFilteredInFootFrame.setToZero();
+ rootJointPosition.set(pelvisPosition);
+ if (pelvisLinearVelocity != null)
+ rootJointLinearVelocity.set(pelvisLinearVelocity);
+ else
+ rootJointLinearVelocity.setToZero();
}
-
- private final FrameVector3D tempFrameVector = new FrameVector3D();
-
- private void updateTrustedFootPosition(boolean trustCoPAsNonSlippingContactPoint, FramePoint3DReadOnly rootJointPosition)
+ else
{
- if (trustCoPAsNonSlippingContactPoint)
+ averagedTrustedFootVelocity.setToZero();
+ for (int i = 0; i < trustedFeet.size(); i++)
{
- tempFrameVector.setIncludingFrame(rootJointPosition);
- tempFrameVector.sub(footToRootJointPosition); // New foot position
- tempFrameVector.sub(footPositionInWorld); // Delta from previous to new foot position
- copPositionInWorld.add(tempFrameVector); // New CoP position
- }
+ SingleFootEstimator footEstimator = footEstimatorMap.get(trustedFeet.get(i));
- footPositionInWorld.set(rootJointPosition);
- footPositionInWorld.sub(footToRootJointPosition);
- }
+ double scaleFactor = 1.0 / trustedFeet.size();
- private final FramePoint2D tempCoP2d = new FramePoint2D();
- private final FrameVector3D tempCoPOffset = new FrameVector3D();
+ // The root joint position is zeroed when we call #updateKinematics
+ rootJointPosition.scaleAdd(scaleFactor, footEstimator.getRootJointPositionFromKinematics(), rootJointPosition);
+ averagedTrustedFootVelocity.scaleAdd(scaleFactor, footEstimator.getCopVelocityInWorld(), averagedTrustedFootVelocity);
- /**
- * Compute the foot CoP. The CoP is the point on the support foot trusted to be not slipping.
- *
- * @param trustedSide
- * @param footSwitch
- */
- private void updateCoPPosition(boolean trustCoPAsNonSlippingContactPoint, boolean useControllerDesiredCoP)
- {
- if (trustCoPAsNonSlippingContactPoint)
- {
- if (useControllerDesiredCoP)
- centerOfPressureDataHolderFromController.getCenterOfPressure(tempCoP2d, foot);
- else
- footSwitch.getCenterOfPressure(tempCoP2d);
-
- if (tempCoP2d.containsNaN())
- {
- tempCoP2d.setToZero(soleFrame);
- }
- else
- {
- boolean isCoPInsideFoot = footPolygon.isPointInside(tempCoP2d);
- if (!isCoPInsideFoot)
- {
- if (footSwitch.getFootLoadPercentage() > 0.2)
- {
- footCenterCoPLineSegment.set(soleFrame, 0.0, 0.0, tempCoP2d.getX(), tempCoP2d.getY());
- int intersections = footPolygon.intersectionWith(footCenterCoPLineSegment, intersectionPoints[0], intersectionPoints[1]);
-
- if (intersections == 0)
- {
- LogTools.info("Found no solution for the CoP projection.");
- tempCoP2d.setToZero(soleFrame);
- }
- else
- {
- tempCoP2d.set(intersectionPoints[0]);
-
- if (intersections == 2)
- LogTools.info("Found two solutions for the CoP projection.");
- }
- }
- else // If foot barely loaded and actual CoP outside, then don't update the raw CoP right below
- {
- tempCoP2d.setIncludingFrame(copRawInFootFrame);
- }
- }
- }
-
- copRawInFootFrame.set(tempCoP2d);
-
- tempCoPOffset.setIncludingFrame(soleFrame, copFilteredInFootFrame.getX(), copFilteredInFootFrame.getY(), 0.0);
- copFilteredInFootFrame.update();
- tempCoPOffset.setIncludingFrame(soleFrame,
- copFilteredInFootFrame.getX() - tempCoPOffset.getX(),
- copFilteredInFootFrame.getY() - tempCoPOffset.getY(),
- 0.0);
-
- tempCoPOffset.changeFrame(worldFrame);
- copPositionInWorld.add(tempCoPOffset);
- }
- else
- {
- tempCoP2d.setToZero(soleFrame);
}
- }
-
- /**
- * Assuming the CoP is not moving, the foot position can be updated. That way we can see if the foot
- * is on the edge.
- *
- * @param plantedFoot
- */
- private void correctFootPositionsUsingCoP(boolean trustCoPAsNonSlippingContactPoint)
- {
- if (!trustCoPAsNonSlippingContactPoint)
- return;
-
- tempCoPOffset.setIncludingFrame(copFilteredInFootFrame, 0.0);
- tempCoPOffset.changeFrame(worldFrame);
- footPositionInWorld.sub(copPositionInWorld, tempCoPOffset);
- }
-
- /**
- * Updates the different kinematics related stuff that is used to estimate the pelvis state
- */
- public void updateKinematics()
- {
- tempFramePoint.setToZero(rootJointFrame);
- tempFramePoint.changeFrame(soleFrame);
- tempFrameVector.setIncludingFrame(tempFramePoint);
- tempFrameVector.changeFrame(worldFrame);
-
- footToRootJointPosition.update(tempFrameVector);
- }
-
- private final Twist tempRootBodyTwist = new Twist();
- private final Twist footTwistInWorld = new Twist();
-
- private void updateFootLinearVelocityInWorld(TwistReadOnly rootBodyTwist)
- {
- computeFootLinearVelocityInWorld(rootBodyTwist, footVelocityInWorld);
+ // We want the average velocity of the feet to trend to zero, since the trusted feet are assumed to not be moving.
+ rootJointLinearVelocity.scaleAdd(-alphaRootJointLinearVelocity.getValue(),
+ averagedTrustedFootVelocity,
+ rootJointLinearVelocity);
}
- private void computeFootLinearVelocityInWorld(TwistReadOnly rootBodyTwist, FixedFrameVector3DBasics footLinearVelocityToPack)
- {
- tempRootBodyTwist.setIncludingFrame(rootBodyTwist);
- tempRootBodyTwist.setBaseFrame(worldFrame);
- tempRootBodyTwist.changeFrame(foot.getBodyFixedFrame());
-
- foot.getBodyFixedFrame().getTwistRelativeToOther(rootJointFrame, footTwistInWorld);
- footTwistInWorld.add(tempRootBodyTwist);
- footTwistInWorld.setBodyFrame(soleFrame);
- footTwistInWorld.changeFrame(worldFrame);
+ rootJointLinearVelocityFDDebug.update();
- footTwistInWorld.getLinearVelocityAt(copPositionInWorld, footLinearVelocityToPack);
- }
+ kinematicsIsUpToDate.set(false);
}
- @Override
- public YoGraphicDefinition getSCS2YoGraphics()
+ public FramePoint3DReadOnly getPelvisPosition()
{
- if (!VISUALIZE)
- return null;
+ return rootJointPosition;
+ }
- YoGraphicGroupDefinition group = new YoGraphicGroupDefinition(getClass().getSimpleName());
- for (SingleFootEstimator footEstimator : footEstimators)
- {
- group.addChild(footEstimator.getSCS2YoGraphics());
- }
- return group;
+ public FrameVector3DReadOnly getPelvisVelocity()
+ {
+ return rootJointLinearVelocity;
}
}
diff --git a/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/PelvisLinearStateUpdater.java b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/PelvisLinearStateUpdater.java
index 1c3615be2e70..5c2c9821db96 100644
--- a/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/PelvisLinearStateUpdater.java
+++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/PelvisLinearStateUpdater.java
@@ -1,6 +1,7 @@
package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation;
import java.util.ArrayList;
+import java.util.HashMap;
import java.util.LinkedHashMap;
import java.util.List;
import java.util.Map;
@@ -11,6 +12,9 @@
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.referenceFrame.interfaces.FixedFramePoint3DBasics;
import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameVector3DBasics;
+import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly;
+import us.ihmc.euclid.referenceFrame.tools.EuclidFrameFactories;
+import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly;
import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry;
import us.ihmc.humanoidRobotics.model.CenterOfPressureDataHolder;
@@ -56,6 +60,7 @@
public class PelvisLinearStateUpdater implements SCS2YoGraphicHolder
{
private static final boolean MORE_YOVARIABLES = false;
+ private static final boolean VISUALIZE_FEET_ESTIMATORS = true;
private static final double minForceZInPercentThresholdToFilterFoot = 0.0;
private static final double maxForceZInPercentThresholdToFilterFoot = 0.45;
@@ -94,11 +99,15 @@ public class PelvisLinearStateUpdater implements SCS2YoGraphicHolder
private final DoubleProvider forceZInPercentThresholdToTrustFoot;
private final DoubleProvider forceZInPercentThresholdToNotTrustFoot;
+ private final SingleFootEstimator[] footEstimators;
+ private final Map footEstimatorMap = new HashMap<>();
+
private final Map footSwitches;
private final Map footForces = new LinkedHashMap<>();
private final Map footWrenches = new LinkedHashMap<>();
private final DoubleProvider delayTimeBeforeTrustingFoot;
private final Map haveFeetHitGroundFiltered = new LinkedHashMap<>();
+ private final Map areFeetNotMovingFiltered = new LinkedHashMap<>();
private final Map areFeetTrusted = new LinkedHashMap<>();
private final Map wereFeetTrustedLastTick = new LinkedHashMap<>();
private final List listOfTrustedFeet = new ArrayList<>();
@@ -166,13 +175,71 @@ public PelvisLinearStateUpdater(FullInverseDynamicsStructure inverseDynamicsStru
setupBunchOfVariables();
- kinematicsBasedLinearStateCalculator = new PelvisKinematicsBasedLinearStateCalculator(inverseDynamicsStructure,
- feetContactablePlaneBodies,
- footSwitches,
- centerOfPressureDataHolderFromController,
+ Map footIMUs = new HashMap<>();
+ for (IMUSensorReadOnly imu : imuProcessedOutputs)
+ {
+ if (feetContactablePlaneBodies.containsKey(imu.getMeasurementLink()))
+ footIMUs.put(imu.getMeasurementLink(), imu);
+ }
+
+ FrameVector3DReadOnly gravityVector = EuclidFrameFactories.newLinkedFrameVector3DReadOnly(() -> worldFrame, new Vector3D(0, 0, -Math.abs(gravitationalAcceleration)));
+
+ DoubleProvider footToRootJointPositionBreakFrequency = new DoubleParameter("FootToRootJointPositionBreakFrequency",
+ registry,
+ stateEstimatorParameters.getKinematicsPelvisPositionFilterFreqInHertz());
+ DoubleProvider copFilterBreakFrequency = new DoubleParameter("CopFilterBreakFrequency", registry, stateEstimatorParameters.getCoPFilterFreqInHertz());
+ YoBoolean useFootIMUData = new YoBoolean("useFootIMUData", registry);
+ YoDouble footLinearVelocityMovingThreshold = new YoDouble("footLinearVelocityMovingThreshold", registry);
+ YoDouble footAngularVelocityMovingThreshold = new YoDouble("footAngularVelocityMovingThreshold", registry);
+ YoDouble footAlphaLeakIMUOnly = new YoDouble("footIMUOnlyAlphaLeak", registry);
+ YoDouble footImuAgainstKinematicsForVelocityBreakFrequency = new YoDouble("footIMUAgainstKinematicsForVelocityBreakFrequency", registry);
+ YoDouble footImuAgainstKinematicsForPositionBreakFrequency = new YoDouble("footIMUAgainstKinematicsForPositionBreakFrequency", registry);
+ footImuAgainstKinematicsForPositionBreakFrequency.set(12.0);
+ footImuAgainstKinematicsForVelocityBreakFrequency.set(2.0);
+ footAlphaLeakIMUOnly.set(0.999);
+ footLinearVelocityMovingThreshold.set(0.3);
+ footAngularVelocityMovingThreshold.set(0.6);
+
+ RigidBodyBasics[] feetRigidBodies = feetContactablePlaneBodies.keySet().toArray(new RigidBodyBasics[0]);
+
+ footEstimators = new SingleFootEstimator[feetRigidBodies.length];
+ for (int i = 0; i < feetRigidBodies.length; i++)
+ {
+ RigidBodyBasics footRigidBody = feetRigidBodies[i];
+ ContactablePlaneBody contactableFoot = feetContactablePlaneBodies.get(footRigidBody);
+ FootSwitchInterface footSwitch = footSwitches.get(footRigidBody);
+ footEstimators[i] = new SingleFootEstimator(rootJoint,
+ contactableFoot,
+ footSwitch,
+ footIMUs.get(footRigidBody),
+ imuBiasProvider,
+ footToRootJointPositionBreakFrequency,
+ copFilterBreakFrequency,
+ centerOfPressureDataHolderFromController,
+ cancelGravityFromAccelerationMeasurement,
+ footLinearVelocityMovingThreshold,
+ footAngularVelocityMovingThreshold,
+ gravityVector,
+ useFootIMUData,
+ footImuAgainstKinematicsForPositionBreakFrequency,
+ footImuAgainstKinematicsForVelocityBreakFrequency,
+ footAlphaLeakIMUOnly,
+ estimatorDT,
+ registry);
+ footEstimatorMap.put(footRigidBody, footEstimators[i]);
+ }
+
+ if (VISUALIZE_FEET_ESTIMATORS)
+ {
+ for (SingleFootEstimator footEstimator : footEstimators)
+ {
+ footEstimator.createVisualization(yoGraphicsListRegistry);
+ }
+ }
+
+ kinematicsBasedLinearStateCalculator = new PelvisKinematicsBasedLinearStateCalculator(footEstimatorMap,
estimatorDT,
stateEstimatorParameters,
- yoGraphicsListRegistry,
registry);
imuBasedLinearStateCalculator = new PelvisIMUBasedLinearStateCalculator(rootJoint,
@@ -263,8 +330,11 @@ private void setupBunchOfVariables()
String footPrefix = foot.getName();
final GlitchFilteredYoBoolean hasFootHitTheGroundFiltered = new GlitchFilteredYoBoolean("has" + footPrefix + "FootHitGroundFiltered", registry, 0);
+ final GlitchFilteredYoBoolean isFootMovingFiltered = new GlitchFilteredYoBoolean("is" + footPrefix + "FootNotMovingFiltered", registry, 0);
hasFootHitTheGroundFiltered.set(true);
+ isFootMovingFiltered.set(false);
haveFeetHitGroundFiltered.put(foot, hasFootHitTheGroundFiltered);
+ areFeetNotMovingFiltered.put(foot, isFootMovingFiltered);
YoBoolean isFootTrusted = new YoBoolean("is" + footPrefix + "FootTrusted", registry);
YoBoolean wasFootTrusted = new YoBoolean("was" + footPrefix + "FootTrustedLastTick", registry);
@@ -322,6 +392,16 @@ private void initializeRobotState()
rootJointVelocity.setToZero();
kinematicsBasedLinearStateCalculator.initialize(rootJointPosition);
+ // Initialize the feet states
+ tempRootBodyTwist.setIncludingFrame(rootJoint.getJointTwist());
+
+ for (SingleFootEstimator footEstimator : footEstimators)
+ {
+ footEstimator.initialize();
+ footEstimator.updateStateFromKinematics(tempRootBodyTwist);
+ footEstimator.correctFootPositionFromRootJoint(false, false, rootJointPosition);
+ }
+
imuBasedLinearStateCalculator.initialize();
mainIMULinearVelocityEstimate.getPositionEstimation().setToZero();
mainIMULinearVelocityEstimate.getRateEstimation().setToZero();
@@ -340,7 +420,13 @@ public void updateForFrozenState()
// Keep setting the position so the localization updater works properly.
rootJoint.setJointPosition(rootJointPosition);
kinematicsBasedLinearStateCalculator.updateKinematics();
- kinematicsBasedLinearStateCalculator.updateNoTrustedFeet(rootJointPosition, null);
+ updateFeetFromKinematics();
+ for (SingleFootEstimator footEstimator : footEstimators)
+ {
+ footEstimator.correctFootPositionFromRootJoint(false, false, rootJointPosition);
+ }
+ kinematicsBasedLinearStateCalculator.setRootJointState(rootJointPosition, null);
+
// Reset the IMU updater
imuBasedLinearStateCalculator.initialize();
@@ -350,14 +436,51 @@ public void updateForFrozenState()
rootJointVelocity.setToZero();
}
+ private final Twist tempRootBodyTwist = new Twist();
+
+ private void updateFeetFromKinematics()
+ {
+ // FIXME do we want to use the fused pelvis velocity?
+ tempRootBodyTwist.setIncludingFrame(rootJoint.getJointTwist());
+ tempRootBodyTwist.getLinearPart().setMatchingFrame(kinematicsBasedLinearStateCalculator.getPelvisVelocity());
+
+ for (SingleFootEstimator footEstimator : footEstimators)
+ {
+ footEstimator.updateStateFromKinematics(tempRootBodyTwist);
+ }
+ }
+
+ private void correctFeetStateFromPelvis()
+ {
+ if (kinematicsBasedLinearStateCalculator.getCorrectTrustedFeetPositions())
+ {
+ for (int i = 0; i < listOfTrustedFeet.size(); i++)
+ {
+ SingleFootEstimator footEstimator = footEstimatorMap.get(listOfTrustedFeet.get(i));
+ footEstimator.correctFootPositionFromRootJoint(true,
+ kinematicsBasedLinearStateCalculator.getTrustCoPAsNonSlippingContactPoint(),
+ kinematicsBasedLinearStateCalculator.getPelvisPosition());
+ }
+ }
+
+ for (int i = 0; i < listOfUnTrustedFeet.size(); i++)
+ {
+ SingleFootEstimator footEstimator = footEstimatorMap.get(listOfUnTrustedFeet.get(i));
+ footEstimator.correctFootPositionFromRootJoint(false, false, kinematicsBasedLinearStateCalculator.getPelvisPosition());
+ }
+ }
+
+
+
public void updateRootJointPositionAndLinearVelocity()
{
+ kinematicsBasedLinearStateCalculator.updateKinematics();
+ updateFeetFromKinematics();
+
if (requestStopEstimationOfPelvisLinearState.getBooleanValue())
return;
- kinematicsBasedLinearStateCalculator.updateKinematics();
-
- numberOfEndEffectorsTrusted.set(setTrustedFeetUsingFootSwitches());
+ numberOfEndEffectorsTrusted.set(setTrustedFeetUsingFootSwitchesAndKinematics());
numberOfEndEffectorsFilteredByLoad.set(0);
if (numberOfEndEffectorsTrusted.getIntegerValue() >= optimalNumberOfTrustedFeet.getValue())
@@ -393,15 +516,21 @@ public void updateRootJointPositionAndLinearVelocity()
tempTwist.changeFrame(rootJointFrame);
rootJointNewLinearVelocityEstimate.setMatchingFrame(tempTwist.getLinearPart());
rootJointPositionEstimate.update(null, rootJointNewLinearVelocityEstimate);
- kinematicsBasedLinearStateCalculator.updateNoTrustedFeet(rootJointPosition, rootJointVelocity);
+
+ for (SingleFootEstimator footEstimator : footEstimators)
+ {
+ footEstimator.correctFootPositionFromRootJoint(false, false, rootJointPosition);
+ }
+
+ kinematicsBasedLinearStateCalculator.setRootJointState(rootJointPosition, rootJointVelocity);
}
else
throw new RuntimeException("No foot trusted!");
}
else if (numberOfEndEffectorsTrusted.getIntegerValue() > 0)
{
- updateTrustedFeetLists();
- kinematicsBasedLinearStateCalculator.estimatePelvisLinearState(listOfTrustedFeet, listOfUnTrustedFeet, rootJointPosition);
+ updateTrustedFeet();
+ kinematicsBasedLinearStateCalculator.estimatePelvisLinearState(listOfTrustedFeet, rootJointPosition, rootJointVelocity);
if (imuBasedLinearStateCalculator.isEstimationEnabled())
{
@@ -412,6 +541,9 @@ else if (numberOfEndEffectorsTrusted.getIntegerValue() > 0)
rootJointPosition.set(kinematicsBasedLinearStateCalculator.getPelvisPosition());
rootJointVelocity.set(kinematicsBasedLinearStateCalculator.getPelvisVelocity());
}
+
+ // FIXME should this used the fused measurement, rather than the kinematic only measurement?
+ correctFeetStateFromPelvis();
}
else
{
@@ -430,8 +562,9 @@ private void updateRootJoint()
imuBasedLinearStateCalculator.saveMeasurementFrameTwist(rootJoint.getJointTwist());
}
- private int setTrustedFeetUsingFootSwitches()
+ private int setTrustedFeetUsingFootSwitchesAndKinematics()
{
+ int numberOfEndEffectorsHitGround = 0;
int numberOfEndEffectorsTrusted = 0;
int windowSize = (int) (delayTimeBeforeTrustingFoot.getValue() / estimatorDT);
@@ -440,19 +573,49 @@ private int setTrustedFeetUsingFootSwitches()
{
RigidBodyBasics foot = feet.get(i);
wereFeetTrustedLastTick.get(foot).set(areFeetTrusted.get(foot).getValue());
- haveFeetHitGroundFiltered.get(foot).setWindowSize(windowSize);
+
+ GlitchFilteredYoBoolean hasFootHitGround = haveFeetHitGroundFiltered.get(foot);
+ GlitchFilteredYoBoolean isFootNotMoving = areFeetNotMovingFiltered.get(foot);
+ hasFootHitGround.setWindowSize(windowSize);
+ isFootNotMoving.setWindowSize(windowSize);
if (footSwitches.get(foot).hasFootHitGroundFiltered())
- haveFeetHitGroundFiltered.get(foot).update(true);
+ hasFootHitGround.update(true);
+ else
+ hasFootHitGround.set(false);
+ if (!footEstimatorMap.get(foot).isFootMoving())
+ isFootNotMoving.update(true);
else
- haveFeetHitGroundFiltered.get(foot).set(false);
+ isFootNotMoving.set(false);
- if (haveFeetHitGroundFiltered.get(foot).getBooleanValue())
+ if (hasFootHitGround.getBooleanValue())
+ numberOfEndEffectorsHitGround++;
+ if (hasFootHitGround.getBooleanValue() && isFootNotMoving.getBooleanValue())
numberOfEndEffectorsTrusted++;
}
- // Update only if at least one foot hit the ground
+ // TODO better incorporate the conditions of nothing moving
+
+ // Update only if at least one foot hit the ground and its not moving
if (numberOfEndEffectorsTrusted > 0)
+ {
+ if (trustOnlyLowestFoot.getValue())
+ {
+ numberOfEndEffectorsTrusted = filterAndTrustLowestFoot();
+ }
+ else
+ {
+ for (int i = 0; i < feet.size(); i++)
+ {
+ RigidBodyBasics foot = feet.get(i);
+ boolean isFootOnGround = haveFeetHitGroundFiltered.get(foot).getBooleanValue();
+ boolean isFootNotMoving = areFeetNotMovingFiltered.get(foot).getBooleanValue();
+ areFeetTrusted.get(foot).set(isFootOnGround && isFootNotMoving);
+ }
+ }
+ }
+ // Update if you have a foot on the ground, but it's moving
+ else if (numberOfEndEffectorsHitGround > 0)
{
if (trustOnlyLowestFoot.getValue())
{
@@ -466,6 +629,8 @@ private int setTrustedFeetUsingFootSwitches()
boolean isFootOnGround = haveFeetHitGroundFiltered.get(foot).getBooleanValue();
areFeetTrusted.get(foot).set(isFootOnGround);
}
+ // override the number of end effectors trusted with the number that hit the ground.
+ numberOfEndEffectorsTrusted = numberOfEndEffectorsHitGround;
}
}
// Else if there is a foot with a force past the threshold trust the force and not the CoP
@@ -494,6 +659,8 @@ private int setTrustedFeetUsingFootSwitches()
}
}
+
+
if (numberOfEndEffectorsTrusted == 0)
{
if (trustImuWhenNoFeetAreInContact.getValue())
@@ -678,7 +845,7 @@ private void computePositionFromMergingMeasurements()
}
}
- private void updateTrustedFeetLists()
+ private void updateTrustedFeet()
{
listOfTrustedFeet.clear();
listOfUnTrustedFeet.clear();
@@ -691,6 +858,32 @@ private void updateTrustedFeetLists()
else
listOfUnTrustedFeet.add(foot);
}
+
+ if (!kinematicsBasedLinearStateCalculator.getKinematicsIsUpToDate())
+ throw new RuntimeException("Leg kinematics needs to be updated before trying to estimate the pelvis position/linear velocity.");
+
+ for (int i = 0; i < listOfTrustedFeet.size(); i++)
+ {
+ SingleFootEstimator footEstimator = footEstimatorMap.get(listOfTrustedFeet.get(i));
+ if (kinematicsBasedLinearStateCalculator.getAssumeTrustedFootAtZeroHeight())
+ {
+ if (kinematicsBasedLinearStateCalculator.getTrustCoPAsNonSlippingContactPoint())
+ {
+ footEstimator.computeStateSettingCoPZ(kinematicsBasedLinearStateCalculator.getTrustCoPAsNonSlippingContactPoint(),
+ kinematicsBasedLinearStateCalculator.getUseControllerDesiredCoP(),
+ 0.0);
+ }
+ else
+ {
+ footEstimator.computeStateSettingFootZ(0.0);
+ }
+ }
+ else
+ {
+ footEstimator.computeState(kinematicsBasedLinearStateCalculator.getTrustCoPAsNonSlippingContactPoint(),
+ kinematicsBasedLinearStateCalculator.getUseControllerDesiredCoP());
+ }
+ }
}
public void getEstimatedPelvisPosition(FramePoint3D pelvisPositionToPack)
@@ -712,8 +905,17 @@ public List getCurrentListOfTrustedFeet()
public YoGraphicDefinition getSCS2YoGraphics()
{
YoGraphicGroupDefinition group = new YoGraphicGroupDefinition(getClass().getSimpleName());
- group.addChild(kinematicsBasedLinearStateCalculator.getSCS2YoGraphics());
group.addChild(imuBasedLinearStateCalculator.getSCS2YoGraphics());
+
+ if (!VISUALIZE_FEET_ESTIMATORS)
+ return group;
+
+ for (SingleFootEstimator footEstimator : footEstimators)
+ {
+ group.addChild(footEstimator.getSCS2YoGraphics());
+ }
+
+
return group;
}
}
diff --git a/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/SingleFootEstimator.java b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/SingleFootEstimator.java
new file mode 100644
index 000000000000..5d149fcd4f6e
--- /dev/null
+++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/SingleFootEstimator.java
@@ -0,0 +1,517 @@
+package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation;
+
+import us.ihmc.euclid.referenceFrame.FrameConvexPolygon2D;
+import us.ihmc.euclid.referenceFrame.FrameLineSegment2D;
+import us.ihmc.euclid.referenceFrame.FramePoint2D;
+import us.ihmc.euclid.referenceFrame.FramePoint3D;
+import us.ihmc.euclid.referenceFrame.FrameVector3D;
+import us.ihmc.euclid.referenceFrame.ReferenceFrame;
+import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameVector3DBasics;
+import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DBasics;
+import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly;
+import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly;
+import us.ihmc.euclid.referenceFrame.interfaces.FrameVertex2DSupplier;
+import us.ihmc.graphicsDescription.appearance.YoAppearance;
+import us.ihmc.graphicsDescription.yoGraphics.YoGraphicPosition;
+import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry;
+import us.ihmc.graphicsDescription.yoGraphics.plotting.YoArtifactPosition;
+import us.ihmc.humanoidRobotics.model.CenterOfPressureDataHolder;
+import us.ihmc.log.LogTools;
+import us.ihmc.mecano.multiBodySystem.interfaces.FloatingJointBasics;
+import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
+import us.ihmc.mecano.spatial.Twist;
+import us.ihmc.mecano.spatial.interfaces.TwistReadOnly;
+import us.ihmc.robotics.SCS2YoGraphicHolder;
+import us.ihmc.robotics.contactable.ContactablePlaneBody;
+import us.ihmc.robotics.sensors.FootSwitchInterface;
+import us.ihmc.scs2.definition.visual.ColorDefinitions;
+import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinition;
+import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinitionFactory;
+import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinitionFactory.DefaultPoint2DGraphic;
+import us.ihmc.sensorProcessing.stateEstimation.IMUSensorReadOnly;
+import us.ihmc.yoVariables.euclid.filters.AlphaFilteredYoFramePoint2D;
+import us.ihmc.yoVariables.euclid.filters.AlphaFilteredYoFrameVector3D;
+import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint2D;
+import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D;
+import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D;
+import us.ihmc.yoVariables.filters.AlphaFilterTools;
+import us.ihmc.yoVariables.providers.BooleanProvider;
+import us.ihmc.yoVariables.providers.DoubleProvider;
+import us.ihmc.yoVariables.registry.YoRegistry;
+import us.ihmc.yoVariables.variable.YoBoolean;
+import us.ihmc.yoVariables.variable.YoDouble;
+
+public class SingleFootEstimator implements SCS2YoGraphicHolder
+{
+ private final RigidBodyBasics foot;
+
+ private final ReferenceFrame rootJointFrame;
+ private final ReferenceFrame soleFrame;
+ private final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
+
+ private final BooleanProvider useIMUData;
+ private final YoFrameVector3D footIKLinearVelocityInWorld;
+ private final YoFrameVector3D footIKAngularVelocityInWorld;
+ private final YoFrameVector3D footIMULinearVelocityInWorld;
+ private final YoFrameVector3D footIMUAngularVelocityInWorld;
+ private final YoFrameVector3D footFusedLinearVelocityInWorld;
+ private final YoFrameVector3D footFusedAngularVelocityInWorld;
+
+ private final YoDouble footLinearVelocity;
+ private final YoDouble footAngularVelocity;
+ private final YoBoolean footIsMoving;
+
+ private final YoFrameVector3D copVelocityInWorld;
+ private final AlphaFilteredYoFrameVector3D footToRootJointPosition;
+ private final AlphaFilteredYoFrameVector3D footToRootJointVelocity;
+ private final YoFramePoint3D footPositionInWorld;
+ /** Debug variable */
+ private final YoFramePoint3D rootJointPositionPerFoot;
+ private final YoFrameVector3D rootJointVelocityPerFoot;
+
+ private final YoFramePoint3D copIKPositionInWorld;
+ private final YoFramePoint3D copIMUPositionInWorld;
+ private final YoFramePoint3D copFusedPositionInWorld;
+ private final YoFramePoint2D previousCoPInFootFrame;
+
+ private final AlphaFilteredYoFramePoint2D copFilteredInFootFrame;
+ private final YoFramePoint2D copRawInFootFrame;
+ private final FrameConvexPolygon2D footPolygon;
+ private final FrameLineSegment2D footCenterCoPLineSegment;
+ private final FootSwitchInterface footSwitch;
+ private final IMUSensorReadOnly footIMU;
+ private final IMUBiasProvider imuBiasProvider;
+ private final CenterOfPressureDataHolder centerOfPressureDataHolderFromController;
+ private final DoubleProvider footAlphaLeakIMUOnly;
+ private final DoubleProvider imuAgainstKinematicsForVelocityBreakFrequency;
+ private final DoubleProvider imuAgainstKinematicsForPositionBreakFrequency;
+ private final BooleanProvider cancelGravityFromAccelerationMeasurement;
+ private final DoubleProvider footLinearVelocityMovingThreshold;
+ private final DoubleProvider footAngularVelocityMovingThreshold;
+ private final FrameVector3DReadOnly gravityVector;
+ private final double estimatorDT;
+
+ private final FramePoint2DBasics[] intersectionPoints = new FramePoint2DBasics[] {new FramePoint2D(), new FramePoint2D()};
+
+ public SingleFootEstimator(FloatingJointBasics rootJoint,
+ ContactablePlaneBody contactableFoot,
+ FootSwitchInterface footSwitch,
+ IMUSensorReadOnly footIMU,
+ IMUBiasProvider imuBiasProvider,
+ DoubleProvider footToRootJointPositionBreakFrequency,
+ DoubleProvider copFilterBreakFrequency,
+ CenterOfPressureDataHolder centerOfPressureDataHolderFromController,
+ BooleanProvider cancelGravityFromAccelerationMeasurement,
+ DoubleProvider footLinearVelocityMovingThreshold,
+ DoubleProvider footAngularVelocityMovingThreshold,
+ FrameVector3DReadOnly gravityVector,
+ BooleanProvider useIMUData,
+ DoubleProvider imuAgainstKinematicsForPositionBreakFrequency,
+ DoubleProvider imuAgainstKinematicsForVelocityBreakFrequency,
+ DoubleProvider footAlphaLeakIMUOnly,
+ double estimatorDT,
+ YoRegistry registry)
+ {
+ this.rootJointFrame = rootJoint.getFrameAfterJoint();
+ this.footSwitch = footSwitch;
+ this.footIMU = footIMU;
+ this.imuBiasProvider = imuBiasProvider;
+ this.centerOfPressureDataHolderFromController = centerOfPressureDataHolderFromController;
+ this.cancelGravityFromAccelerationMeasurement = cancelGravityFromAccelerationMeasurement;
+ this.footLinearVelocityMovingThreshold = footLinearVelocityMovingThreshold;
+ this.footAngularVelocityMovingThreshold = footAngularVelocityMovingThreshold;
+ this.gravityVector = gravityVector;
+ this.useIMUData = useIMUData;
+ this.imuAgainstKinematicsForPositionBreakFrequency = imuAgainstKinematicsForPositionBreakFrequency;
+ this.imuAgainstKinematicsForVelocityBreakFrequency = imuAgainstKinematicsForVelocityBreakFrequency;
+ this.footAlphaLeakIMUOnly = footAlphaLeakIMUOnly;
+ this.estimatorDT = estimatorDT;
+ foot = contactableFoot.getRigidBody();
+ soleFrame = contactableFoot.getContactFrame();
+
+ String namePrefix = foot.getName();
+
+ DoubleProvider alphaFoot = () -> AlphaFilterTools.computeAlphaGivenBreakFrequencyProperly(footToRootJointPositionBreakFrequency.getValue(), estimatorDT);
+ footToRootJointPosition = new AlphaFilteredYoFrameVector3D(namePrefix + "FootToRootJointPosition", "", registry, alphaFoot, worldFrame);
+ footToRootJointVelocity = new AlphaFilteredYoFrameVector3D(namePrefix + "FootToRootJointVelocity", "", registry, alphaFoot, worldFrame);
+ rootJointPositionPerFoot = new YoFramePoint3D(namePrefix + "BasedRootJointPosition", worldFrame, registry);
+ rootJointVelocityPerFoot = new YoFrameVector3D(namePrefix + "BasedRootJointVelocity", worldFrame, registry);
+ footPositionInWorld = new YoFramePoint3D(namePrefix + "FootPositionInWorld", worldFrame, registry);
+ footPolygon = new FrameConvexPolygon2D(FrameVertex2DSupplier.asFrameVertex2DSupplier(contactableFoot.getContactPoints2D()));
+ footCenterCoPLineSegment = new FrameLineSegment2D(soleFrame);
+ copRawInFootFrame = new YoFramePoint2D(namePrefix + "CoPRawInFootFrame", soleFrame, registry);
+ previousCoPInFootFrame = new YoFramePoint2D(namePrefix + "PreviousCoPInFootFrame", soleFrame, registry);
+
+ footLinearVelocity = new YoDouble(namePrefix + "LinearVelocity", registry);
+ footAngularVelocity = new YoDouble(namePrefix + "AngularVelocity", registry);
+ footIsMoving = new YoBoolean(namePrefix + "IsMoving", registry);
+
+ footIKLinearVelocityInWorld = new YoFrameVector3D(namePrefix + "IKLinearVelocityInWorld", worldFrame, registry);
+ footIKAngularVelocityInWorld = new YoFrameVector3D(namePrefix + "IKAngularVelocityInWorld", worldFrame, registry);
+ footIMULinearVelocityInWorld = new YoFrameVector3D(namePrefix + "IMULinearVelocityInWorld", worldFrame, registry);
+ footIMUAngularVelocityInWorld = new YoFrameVector3D(namePrefix + "IMUAngularVelocityInWorld", worldFrame, registry);
+ footFusedLinearVelocityInWorld = new YoFrameVector3D(namePrefix + "FusedLinearVelocityInWorld", worldFrame, registry);
+ footFusedAngularVelocityInWorld = new YoFrameVector3D(namePrefix + "FusedAngularVelocityInWorld", worldFrame, registry);
+
+ DoubleProvider alphaCop = () -> AlphaFilterTools.computeAlphaGivenBreakFrequencyProperly(copFilterBreakFrequency.getValue(), estimatorDT);
+ copFilteredInFootFrame = new AlphaFilteredYoFramePoint2D(namePrefix + "CoPFilteredInFootFrame", "", registry, alphaCop, copRawInFootFrame);
+ copFilteredInFootFrame.update(0.0, 0.0);
+ copIKPositionInWorld = new YoFramePoint3D(namePrefix + "CoPIKPositionInWorld", worldFrame, registry);
+ copIMUPositionInWorld = new YoFramePoint3D(namePrefix + "CoPIMUPositionInWorld", worldFrame, registry);
+ copFusedPositionInWorld = new YoFramePoint3D(namePrefix + "CoPFusedPositionInWorld", worldFrame, registry);
+ copVelocityInWorld = new YoFrameVector3D(namePrefix + "CoPVelocityInWorld", worldFrame, registry);
+ }
+
+ public void createVisualization(YoGraphicsListRegistry yoGraphicsListRegistry)
+ {
+ if (yoGraphicsListRegistry == null)
+ return;
+
+ String sidePrefix = foot.getName();
+ YoGraphicPosition copInWorld = new YoGraphicPosition(sidePrefix + "StateEstimatorCoP", copFusedPositionInWorld, 0.005, YoAppearance.DeepPink());
+ YoArtifactPosition artifact = copInWorld.createArtifact();
+ artifact.setVisible(false);
+ yoGraphicsListRegistry.registerArtifact("StateEstimator", artifact);
+ }
+
+ @Override
+ public YoGraphicDefinition getSCS2YoGraphics()
+ {
+ return YoGraphicDefinitionFactory.newYoGraphicPoint2D(foot.getName() + "StateEstimatorCoP", copFusedPositionInWorld,
+ 0.01,
+ ColorDefinitions.DeepPink(),
+ DefaultPoint2DGraphic.CIRCLE);
+ }
+
+ public void initialize()
+ {
+ footToRootJointPosition.reset();
+ footToRootJointVelocity.reset();
+ copFilteredInFootFrame.reset();
+ copFilteredInFootFrame.update(0.0, 0.0);
+ copFusedPositionInWorld.setMatchingFrame(copFilteredInFootFrame, 0.0);
+ copVelocityInWorld.setToZero();
+ }
+
+ private final FramePoint3D tempFramePoint = new FramePoint3D();
+
+ private final Twist tempRootBodyTwist = new Twist();
+ private final Twist footTwistInWorld = new Twist();
+ private final Twist imuTwist = new Twist();
+
+
+ private final FrameVector3D tempFrameVector = new FrameVector3D();
+ private final FramePoint3D tempPoint = new FramePoint3D();
+
+ /**
+ * Updates the different kinematics related stuff that is used to estimate the pelvis state. This should be called before calling the other objectives.
+ */
+ public void updateStateFromKinematics(TwistReadOnly rootBodyTwist)
+ {
+ computeFootTwistInWorldFusingBaseStateAndFootIMU(rootBodyTwist);
+
+ // Update the vector from the root joint to the foot, based on the kinematics
+ tempFramePoint.setToZero(rootJointFrame);
+ tempFramePoint.changeFrame(soleFrame);
+
+ tempFrameVector.setIncludingFrame(tempFramePoint);
+ tempFrameVector.changeFrame(worldFrame);
+
+ footToRootJointPosition.update(tempFrameVector);
+
+ footLinearVelocity.set(getFootLinearVelocityInWorld().norm());
+ footAngularVelocity.set(getFootAngularVelocityInWorld().norm());
+
+ if (useIMUData.getValue() && footIMU != null)
+ footIsMoving.set(footLinearVelocity.getDoubleValue() > footLinearVelocityMovingThreshold.getValue() || footAngularVelocity.getDoubleValue() > footAngularVelocityMovingThreshold.getValue());
+ else
+ footIsMoving.set(false);
+ }
+
+ /**
+ * One of the options for computing the
+ * @param useControllerDesiredCoP
+ * @param zPosition
+ */
+ public void computeStateSettingCoPZ(boolean trustCoPAsNonSlippingContactPoint, boolean useControllerDesiredCoP, double zPosition)
+ {
+ updateCoPState(trustCoPAsNonSlippingContactPoint, useControllerDesiredCoP);
+ copFusedPositionInWorld.setZ(zPosition);
+ if (trustCoPAsNonSlippingContactPoint)
+ correctFootPositionUsingCoP();
+
+ rootJointPositionPerFoot.add(footPositionInWorld, footToRootJointPosition);
+ rootJointVelocityPerFoot.sub(footFusedLinearVelocityInWorld, footToRootJointVelocity);
+ }
+
+ public void computeState(boolean trustCoPAsNonSlippingContactPoint, boolean useControllerDesiredCoP)
+ {
+ updateCoPState(trustCoPAsNonSlippingContactPoint, useControllerDesiredCoP);
+ if (trustCoPAsNonSlippingContactPoint)
+ correctFootPositionUsingCoP();
+
+ rootJointPositionPerFoot.add(footPositionInWorld, footToRootJointPosition);
+ rootJointVelocityPerFoot.sub(footFusedLinearVelocityInWorld, footToRootJointVelocity);
+ }
+
+ public void computeStateSettingFootZ(double zPosition)
+ {
+ footPositionInWorld.setZ(zPosition);
+ // Update the CoP position from this foot position
+ copIKPositionInWorld.set(footPositionInWorld);
+ copFusedPositionInWorld.set(footPositionInWorld);
+
+ rootJointPositionPerFoot.add(footPositionInWorld, footToRootJointPosition);
+ rootJointVelocityPerFoot.sub(footFusedLinearVelocityInWorld, footToRootJointVelocity);
+ }
+
+ /**
+ * Corrects the position of the foot and center of pressure in the world using the root joint position and the forward kinematics from the root joint to the
+ * foot. If the foot is not trusted, it sets the center of pressure from the foot. If it is trusted, but is non slipping, we shift the center of pressure
+ * based on this correction.
+ *
+ *
+ * This should be called once the fusion of the different feet to compute the root joint state has occurred.
+ *
+ *
+ * @param isTrusted whether the foot is trusted, and therefore the CoP is trusted.
+ * @param trustCoPAsNonSlippingContactPoint whether to shift the CoP of a trusted foot based on the correction
+ * @param rootJointPosition accurate root joint position to apply.
+ */
+ public void correctFootPositionFromRootJoint(boolean isTrusted, boolean trustCoPAsNonSlippingContactPoint, FramePoint3DReadOnly rootJointPosition)
+ {
+ // Store the old, uncorrected foot position.
+ tempPoint.set(footPositionInWorld);
+ // Update the foot position
+ footPositionInWorld.sub(rootJointPosition, footToRootJointPosition);
+
+ if (!isTrusted)
+ {
+ // If the foot isn't trusted, the CoP position isn't relevant, and we can set it to zero in the foot frame.
+ copFusedPositionInWorld.set(footPositionInWorld);
+ copRawInFootFrame.setToZero();
+ copFilteredInFootFrame.setToZero();
+ }
+ else if (trustCoPAsNonSlippingContactPoint)
+ {
+ // If the foot is trusted, but we allow the CoP to move, we need to shift the CoP position to account for this drift. That means computing the drift
+ // delta correction, and then adding it to the position.
+ tempFrameVector.sub(footPositionInWorld, tempPoint); // Delta from previous to new foot position
+ copFusedPositionInWorld.add(tempFrameVector); // New CoP position
+ }
+ }
+
+ public FramePoint3DReadOnly getRootJointPositionFromKinematics()
+ {
+ return rootJointPositionPerFoot;
+ }
+
+ public FrameVector3DReadOnly getRootJointVelocityFromKinematics()
+ {
+ return rootJointVelocityPerFoot;
+ }
+
+ public FrameVector3DReadOnly getCopVelocityInWorld()
+ {
+ return copVelocityInWorld;
+ }
+
+ public FrameVector3DReadOnly getFootLinearVelocityInWorld()
+ {
+ return footFusedLinearVelocityInWorld;
+ }
+
+ public FrameVector3DReadOnly getFootAngularVelocityInWorld()
+ {
+ return footFusedAngularVelocityInWorld;
+ }
+
+ public boolean isFootMoving()
+ {
+ return footIsMoving.getBooleanValue();
+ }
+
+ private final FramePoint2D tempCoP2d = new FramePoint2D();
+ private final FrameVector3D tempCoPOffset = new FrameVector3D();
+
+ private void updateCoPState(boolean trustCoPAsNonSlippingContactPoint, boolean useControllerDesiredCoP)
+ {
+ if (trustCoPAsNonSlippingContactPoint)
+ {
+ if (useControllerDesiredCoP)
+ centerOfPressureDataHolderFromController.getCenterOfPressure(tempCoP2d, foot);
+ else
+ footSwitch.getCenterOfPressure(tempCoP2d);
+
+ if (tempCoP2d.containsNaN())
+ {
+ tempCoP2d.setToZero(soleFrame);
+ }
+ else
+ {
+ boolean isCoPInsideFoot = footPolygon.isPointInside(tempCoP2d);
+ if (!isCoPInsideFoot)
+ {
+ if (footSwitch.getFootLoadPercentage() > 0.2)
+ {
+ footCenterCoPLineSegment.set(soleFrame, 0.0, 0.0, tempCoP2d.getX(), tempCoP2d.getY());
+ int intersections = footPolygon.intersectionWith(footCenterCoPLineSegment, intersectionPoints[0], intersectionPoints[1]);
+
+ if (intersections == 0)
+ {
+ LogTools.info("Found no solution for the CoP projection.");
+ tempCoP2d.setToZero(soleFrame);
+ }
+ else
+ {
+ tempCoP2d.set(intersectionPoints[0]);
+
+ if (intersections == 2)
+ LogTools.info("Found two solutions for the CoP projection.");
+ }
+ }
+ else // If foot barely loaded and actual CoP outside, then don't update the raw CoP right below
+ {
+ tempCoP2d.setIncludingFrame(copRawInFootFrame);
+ }
+ }
+ }
+
+
+ // Update the CoP values.
+ previousCoPInFootFrame.set(copFilteredInFootFrame);
+ copRawInFootFrame.set(tempCoP2d);
+ copFilteredInFootFrame.update();
+
+ // Update the CoP value from kinematics. This treats it as non-stationary.
+ copIKPositionInWorld.setMatchingFrame(soleFrame, copFilteredInFootFrame.getX(), copFilteredInFootFrame.getY(), 0.0);
+
+ // Update the change in the CoP from the previous tick.
+ tempCoPOffset.setIncludingFrame(soleFrame,
+ copFilteredInFootFrame.getX() - previousCoPInFootFrame.getX(),
+ copFilteredInFootFrame.getY() - previousCoPInFootFrame.getY(),
+ 0.0);
+ tempCoPOffset.changeFrame(worldFrame);
+ // Apply this same change to the CoP in world.
+ copFusedPositionInWorld.add(tempCoPOffset);
+ }
+ else
+ {
+ tempCoP2d.setToZero(soleFrame);
+ // Update the CoP value to match the center of the foot.
+ copIKPositionInWorld.setFromReferenceFrame(soleFrame);
+ copFusedPositionInWorld.setFromReferenceFrame(soleFrame);
+ }
+
+ // If we have an IMU and are using it, we should update the fused value.
+ if (useIMUData.getValue() && footIMU != null)
+ {
+ // Integrate the fused position using the velocity, which is from the kinematics and the IMU.
+ computeLinearVelocityAtPointInWorld(copFusedPositionInWorld, tempFrameVector);
+ copIMUPositionInWorld.scaleAdd(footAlphaLeakIMUOnly.getValue() * estimatorDT, tempFrameVector, copFusedPositionInWorld);
+
+ // Fuse the kinematic data with the IMU data. A higher break frequency equals a lower alpha value, trusting the kinematics more. If break frequency is
+ // zero, alpha equals one, and we trust the IMU completely.
+ double alpha = AlphaFilterTools.computeAlphaGivenBreakFrequencyProperly(imuAgainstKinematicsForPositionBreakFrequency.getValue(), estimatorDT);
+ copFusedPositionInWorld.interpolate(copIKPositionInWorld, copIMUPositionInWorld, alpha);
+ }
+
+ // Update the velocity of the foot at the CoP position
+ computeLinearVelocityAtPointInWorld(copFusedPositionInWorld, copVelocityInWorld);
+ }
+
+ private void correctFootPositionUsingCoP()
+ {
+ // Compute where the CoP position is in the world, according to the kinematics
+ tempCoPOffset.setIncludingFrame(copFilteredInFootFrame, 0.0);
+ tempCoPOffset.changeFrame(worldFrame);
+ // Update where the foot must be, according to this CoP location
+ footPositionInWorld.sub(copFusedPositionInWorld, tempCoPOffset);
+ }
+
+ private final FrameVector3D linearAcceleration = new FrameVector3D();
+ private final FrameVector3D angularVelocity = new FrameVector3D();
+
+ private void computeFootTwistInWorldFusingBaseStateAndFootIMU(TwistReadOnly rootBodyTwist)
+ {
+ tempRootBodyTwist.setIncludingFrame(rootBodyTwist);
+ tempRootBodyTwist.setBaseFrame(worldFrame);
+ tempRootBodyTwist.changeFrame(foot.getBodyFixedFrame());
+
+ foot.getBodyFixedFrame().getTwistRelativeToOther(rootJointFrame, footTwistInWorld);
+
+ tempFrameVector.setIncludingFrame(footTwistInWorld.getLinearPart());
+ tempFrameVector.changeFrame(worldFrame);
+ footToRootJointVelocity.update(tempFrameVector);
+
+ footTwistInWorld.add(tempRootBodyTwist);
+ footTwistInWorld.setBodyFrame(soleFrame);
+ footTwistInWorld.changeFrame(worldFrame);
+
+ if (useIMUData.getValue() && footIMU != null)
+ {
+ // Record the kinematic data
+ footIKLinearVelocityInWorld.set(footTwistInWorld.getLinearPart());
+ footIKAngularVelocityInWorld.set(footTwistInWorld.getAngularPart());
+
+ // Get the previous IMU twist estimate from the fused data set
+ imuTwist.setToZero(worldFrame);
+ imuTwist.getLinearPart().set(footFusedLinearVelocityInWorld);
+ imuTwist.getAngularPart().set(footFusedAngularVelocityInWorld);
+ imuTwist.changeFrame(footIMU.getMeasurementFrame());
+
+ // Compute the acceleration reading from the IMU
+ linearAcceleration.setIncludingFrame(footIMU.getMeasurementFrame(), footIMU.getLinearAccelerationMeasurement());
+ FrameVector3DReadOnly accelerationBias = imuBiasProvider.getLinearAccelerationBiasInIMUFrame(footIMU);
+ if (accelerationBias != null)
+ linearAcceleration.sub(accelerationBias);
+
+ // Update acceleration (minus gravity)
+ if (cancelGravityFromAccelerationMeasurement.getValue())
+ {
+ linearAcceleration.changeFrame(worldFrame);
+ linearAcceleration.add(gravityVector);
+ linearAcceleration.changeFrame(footIMU.getMeasurementFrame());
+ }
+
+ // Compute the gyro reading from the IMU
+ angularVelocity.setIncludingFrame(footIMU.getMeasurementFrame(), footIMU.getAngularVelocityMeasurement());
+ FrameVector3DReadOnly gyroBias = imuBiasProvider.getAngularVelocityBiasInIMUFrame(footIMU);
+ if (gyroBias != null)
+ angularVelocity.sub(gyroBias);
+
+ // Integrate the linear acceleration into the velocity measurement
+ imuTwist.getLinearPart().scaleAdd(estimatorDT * footAlphaLeakIMUOnly.getValue(), linearAcceleration, imuTwist.getLinearPart());
+ // Set the angular part from the gyro
+ imuTwist.getAngularPart().set(angularVelocity);
+
+ imuTwist.changeFrame(worldFrame);
+
+ footIMUAngularVelocityInWorld.set(imuTwist.getAngularPart());
+ footIMULinearVelocityInWorld.set(imuTwist.getLinearPart());
+
+ // Fuse the IMU and kinematics data for velocity
+ double alpha = AlphaFilterTools.computeAlphaGivenBreakFrequencyProperly(imuAgainstKinematicsForVelocityBreakFrequency.getValue(), estimatorDT);
+ footFusedAngularVelocityInWorld.interpolate(footIKAngularVelocityInWorld, footIMUAngularVelocityInWorld, alpha);
+ footFusedLinearVelocityInWorld.interpolate(footIKLinearVelocityInWorld, footIMULinearVelocityInWorld, alpha);
+ }
+ else
+ {
+ footIKLinearVelocityInWorld.setToNaN();
+ footIKAngularVelocityInWorld.setToNaN();
+ footIMULinearVelocityInWorld.setToNaN();
+ footIMUAngularVelocityInWorld.setToNaN();
+
+ footFusedLinearVelocityInWorld.set(footTwistInWorld.getLinearPart());
+ footFusedAngularVelocityInWorld.set(footTwistInWorld.getAngularPart());
+ }
+ }
+
+ private void computeLinearVelocityAtPointInWorld(FramePoint3DReadOnly point, FixedFrameVector3DBasics footLinearVelocityToPack)
+ {
+ footTwistInWorld.getLinearPart().set(footFusedLinearVelocityInWorld);
+ footTwistInWorld.getAngularPart().set(footFusedAngularVelocityInWorld);
+
+ footTwistInWorld.getLinearVelocityAt(point, footLinearVelocityToPack);
+ }
+}
diff --git a/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/MeasurementModel.java b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/MeasurementModel.java
new file mode 100644
index 000000000000..bbbbdb11f499
--- /dev/null
+++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/MeasurementModel.java
@@ -0,0 +1,213 @@
+package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF;
+
+import org.ejml.data.DMatrixRMaj;
+import org.ejml.dense.row.CommonOps_DDRM;
+import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly;
+import us.ihmc.euclid.tools.QuaternionTools;
+import us.ihmc.euclid.tuple3D.Vector3D;
+import us.ihmc.euclid.tuple4D.Quaternion;
+import us.ihmc.matrixlib.MatrixTools;
+import us.ihmc.yoVariables.euclid.YoQuaternion;
+import us.ihmc.yoVariables.providers.DoubleProvider;
+import us.ihmc.yoVariables.registry.YoRegistry;
+import us.ihmc.yoVariables.variable.YoBoolean;
+import us.ihmc.yoVariables.variable.YoDouble;
+
+import static us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF.OdometryIndexHelper.*;
+
+class MeasurementModel
+{
+ private static final DMatrixRMaj eye3x3 = CommonOps_DDRM.identity(3);
+
+ private final StateVariables baseState;
+ private final StateVariables footState;
+ private final SensedVariables footSensing;
+ private final MeasurementVariables predictedMeasurements;
+ private final FrameVector3DReadOnly gravityVector;
+
+ // State variables
+ public final YoQuaternion footOrientationInBaseFrame;
+
+ public final YoDouble footVariance;
+ private final DoubleProvider contactThreshold;
+
+ // Temp variables
+ private final Quaternion footOrientationError = new Quaternion();
+ private final Vector3D positionError = new Vector3D();
+ private final Vector3D velocityError = new Vector3D();
+ private final Vector3D tempVector = new Vector3D();
+ private final DMatrixRMaj rotationMatrixTranspose = new DMatrixRMaj(3, 3);
+ private final DMatrixRMaj skewMatrix = new DMatrixRMaj(3, 3);
+ private final DMatrixRMaj left3x3 = new DMatrixRMaj(3, 3);
+ private final DMatrixRMaj left = new DMatrixRMaj(4, 4);
+ private final DMatrixRMaj right = new DMatrixRMaj(4, 4);
+ private final DMatrixRMaj lr = new DMatrixRMaj(4, 4);
+ private final Quaternion tempRotation = new Quaternion();
+
+ public MeasurementModel(String prefix,
+ StateVariables baseState,
+ StateVariables footState,
+ SensedVariables footSensing,
+ MeasurementVariables predictedMeasurement,
+ FrameVector3DReadOnly gravityVector,
+ DoubleProvider contactThreshold,
+ YoRegistry registry)
+ {
+ this.baseState = baseState;
+ this.footState = footState;
+ this.footSensing = footSensing;
+ this.predictedMeasurements = predictedMeasurement;
+ this.gravityVector = gravityVector;
+ this.contactThreshold = contactThreshold;
+
+ footOrientationInBaseFrame = new YoQuaternion(prefix + "OrientationInBaseFrame", registry);
+ footVariance = new YoDouble(prefix + "FootVariance", registry);
+ }
+
+ /**
+ * This is equation 35
+ */
+ public void update(DMatrixRMaj footCovariance)
+ {
+ footSensing.isInContact.set(computeIsFootInContact(footCovariance));
+
+ // First row. Update the predicted foot position of the foot relative to the base in the base frame
+ predictedMeasurements.relativePosition.sub(footState.translation, baseState.translation);
+ baseState.orientation.inverseTransform(predictedMeasurements.relativePosition);
+
+ // Update the predicted foot orientation of the foot relative to the base in the base frame
+ QuaternionTools.multiplyConjugateLeft(baseState.orientation, footState.orientation, footOrientationInBaseFrame);
+
+ // Compute the error between the predicted and measured foot orientation
+ QuaternionTools.multiplyConjugateLeft(footSensing.orientationMeasurement, footOrientationInBaseFrame, footOrientationError);
+ footOrientationError.getRotationVector(predictedMeasurements.relativeOrientationError);
+
+ // Third row. Compute the linear velocity error in the base frame
+ velocityError.sub(baseState.linearVelocity, footState.linearVelocity);
+ baseState.orientation.inverseTransform(velocityError);
+
+ predictedMeasurements.relativeLinearVelocityError.cross(baseState.unbiasedGyro, footSensing.positionMeasurement);
+ predictedMeasurements.relativeLinearVelocityError.scale(-1.0);
+ predictedMeasurements.relativeLinearVelocityError.sub(velocityError);
+ predictedMeasurements.relativeLinearVelocityError.add(footSensing.linearVelocity);
+
+ // Fourth row. Set the predicted linear velocity
+ predictedMeasurements.contactVelocity.set(footState.linearVelocity);
+
+ // Fifth row. Set the predicted gravity vector
+ footState.orientation.inverseTransform(gravityVector, predictedMeasurements.accelMeasure);
+ if (OdometryKalmanFilter.includeBias)
+ predictedMeasurements.accelMeasure.add(footState.accelBias);
+ }
+
+ private final DMatrixRMaj vector = new DMatrixRMaj(3, 1);
+ private final DMatrixRMaj tempDVector = new DMatrixRMaj(3, 1);
+ private final DMatrixRMaj tempScalar = new DMatrixRMaj(3, 1);
+
+ private boolean computeIsFootInContact(DMatrixRMaj footCovariance)
+ {
+ footState.linearVelocity.get(vector);
+
+ CommonOps_DDRM.mult(footCovariance, vector, tempDVector);
+ CommonOps_DDRM.multTransA(vector, tempDVector, tempScalar);
+
+ footVariance.set(Math.sqrt(tempScalar.get(0, 0)));
+
+ // tests the mahalonobis distance of the foot velocity being below a certain threshold.
+ return footVariance.getValue() < contactThreshold.getValue();
+ }
+
+ public boolean getIsFootInContact()
+ {
+ return footSensing.isInContact.getBooleanValue();
+ }
+
+ /**
+ * This is equation 41
+ */
+ public void computeBaseMeasurementJacobian(int rowOffset, DMatrixRMaj jacobianToPack)
+ {
+ OdometryTools.toRotationMatrix(baseState.orientation, rotationMatrixTranspose);
+ CommonOps_DDRM.transpose(rotationMatrixTranspose);
+
+ positionError.sub(footState.translation, baseState.translation);
+ velocityError.sub(footState.linearVelocity, baseState.linearVelocity);
+ baseState.orientation.inverseTransform(positionError);
+ baseState.orientation.inverseTransform(velocityError);
+
+ OdometryTools.toSkewSymmetricMatrix(positionError, skewMatrix);
+
+ // First row. Partial of relative foot position w.r.t. the base state
+ int row = rowOffset + measurementRelativeTranslationIndex;
+ MatrixTools.setMatrixBlock(jacobianToPack, row, errorTranslationIndex, rotationMatrixTranspose, 0, 0, 3, 3, -1.0);
+ CommonOps_DDRM.insert(skewMatrix, jacobianToPack, row, errorOrientationIndex);
+
+ // Second row. Partial of relative foot orientation error w.r.t. the base state
+ row = rowOffset + measurementRelativeOrientationErrorIndex;
+ QuaternionTools.multiplyConjugateLeft(footState.orientation, baseState.orientation, tempRotation);
+ OdometryTools.lOperator(tempRotation, left);
+ OdometryTools.rOperator(footSensing.orientationMeasurement, right);
+ CommonOps_DDRM.mult(left, right, lr);
+ MatrixTools.setMatrixBlock(jacobianToPack, row, errorOrientationIndex, lr, 1, 1, 3, 3, -1.0);
+
+ // Third row. Partial of relative foot velocity error w.r.t. the base state
+ row = rowOffset + measurementRelativeVelocityIndex;
+ OdometryTools.toSkewSymmetricMatrix(velocityError, skewMatrix);
+
+ MatrixTools.setMatrixBlock(jacobianToPack, row, errorLinearVelocityIndex, rotationMatrixTranspose, 0, 0, 3, 3, -1.0);
+ CommonOps_DDRM.insert(skewMatrix, jacobianToPack, row, errorOrientationIndex);
+
+ if (OdometryKalmanFilter.includeBias)
+ {
+ OdometryTools.toSkewSymmetricMatrix(footSensing.positionMeasurement, skewMatrix);
+ MatrixTools.setMatrixBlock(jacobianToPack, row, errorGyroBiasIndex, skewMatrix, 0, 0, 3, 3, -1.0);
+ }
+ }
+
+ /**
+ * This is equation 42
+ */
+ public void computeFootMeasurementJacobian(int rowOffset, int colOffset, DMatrixRMaj jacobianToPack)
+ {
+ OdometryTools.toRotationMatrix(baseState.orientation, rotationMatrixTranspose);
+ CommonOps_DDRM.transpose(rotationMatrixTranspose);
+
+ positionError.sub(footState.translation, baseState.translation);
+ velocityError.sub(footState.linearVelocity, baseState.linearVelocity);
+ baseState.orientation.inverseTransform(positionError);
+ baseState.orientation.inverseTransform(velocityError);
+
+ OdometryTools.toSkewSymmetricMatrix(positionError, skewMatrix);
+
+ // First row. Partial of relative contact position w.r.t. foot state
+ int row = rowOffset + measurementRelativeTranslationIndex;
+ CommonOps_DDRM.insert(rotationMatrixTranspose, jacobianToPack, row, colOffset + errorTranslationIndex);
+
+ // Second row. Partial of relative orientation error w.r.t. foot state
+ row = rowOffset + measurementRelativeOrientationErrorIndex;
+ tempRotation.set(footSensing.orientationMeasurement);
+ tempRotation.multiplyConjugateOther(baseState.orientation);
+ tempRotation.multiply(footState.orientation);
+ OdometryTools.l3Operator(tempRotation, left3x3);
+ CommonOps_DDRM.insert(left3x3, jacobianToPack, row, colOffset + errorOrientationIndex);
+
+ // Third row. Partial of relative velocity error w.r.t. foot state
+ row = rowOffset + measurementRelativeVelocityIndex;
+ CommonOps_DDRM.insert(rotationMatrixTranspose, jacobianToPack, row, colOffset + errorLinearVelocityIndex);
+
+ // Fourth row. Partial of contact velocity w.r.t. foot state
+ // The contact velocity is directly the foot velocity when it's in contact.
+ row = rowOffset + measurementContactVelocityIndex;
+ CommonOps_DDRM.insert(eye3x3, jacobianToPack, row, colOffset + errorLinearVelocityIndex);
+
+ // Fifth row. Partial of contact acceleration w.r.t. foot state
+ row = rowOffset + measurementAccelIndex;
+ tempVector.set(gravityVector);
+ footState.orientation.inverseTransform(tempVector);
+ OdometryTools.toSkewSymmetricMatrix(tempVector, skewMatrix);
+ CommonOps_DDRM.insert(skewMatrix, jacobianToPack, row, colOffset + errorOrientationIndex);
+
+ if (OdometryKalmanFilter.includeBias)
+ CommonOps_DDRM.insert(eye3x3, jacobianToPack, row, colOffset + errorAccelBiasIndex);
+ }
+}
diff --git a/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/MeasurementResidualVariables.java b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/MeasurementResidualVariables.java
new file mode 100644
index 000000000000..debe6134a654
--- /dev/null
+++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/MeasurementResidualVariables.java
@@ -0,0 +1,35 @@
+package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF;
+
+import org.ejml.data.DMatrixRMaj;
+import us.ihmc.yoVariables.euclid.YoVector3D;
+import us.ihmc.yoVariables.registry.YoRegistry;
+
+import static us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF.OdometryIndexHelper.*;
+
+class MeasurementResidualVariables
+{
+ // State variables
+ public final YoVector3D footPositionResidual;
+ public final YoVector3D footOrientationResidual;
+ public final YoVector3D linearVelocityResidual;
+ public final YoVector3D contactVelocityResidual;
+ public final YoVector3D contactAccelResidual;
+
+ public MeasurementResidualVariables(String prefix, YoRegistry registry)
+ {
+ footPositionResidual = new YoVector3D(prefix + "PositionResidual", registry);
+ footOrientationResidual = new YoVector3D(prefix + "OrientationResidual", registry);
+ linearVelocityResidual = new YoVector3D(prefix + "LinearVelocityResidual", registry);
+ contactVelocityResidual = new YoVector3D(prefix + "ContactVelocityResidual", registry);
+ contactAccelResidual = new YoVector3D(prefix + "ContactAccelResidual", registry);
+ }
+
+ public void set(int start, DMatrixRMaj residual)
+ {
+ footPositionResidual.set(start + measurementRelativeTranslationIndex, residual);
+ footOrientationResidual.set(start + measurementRelativeOrientationErrorIndex, residual);
+ linearVelocityResidual.set(start + measurementRelativeVelocityIndex, residual);
+ contactVelocityResidual.set(start + measurementContactVelocityIndex, residual);
+ contactAccelResidual.set(start + measurementAccelIndex, residual);
+ }
+}
diff --git a/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/MeasurementVariables.java b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/MeasurementVariables.java
new file mode 100644
index 000000000000..d01c4de05a50
--- /dev/null
+++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/MeasurementVariables.java
@@ -0,0 +1,38 @@
+package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF;
+
+import org.ejml.data.DMatrixRMaj;
+import us.ihmc.yoVariables.euclid.YoVector3D;
+import us.ihmc.yoVariables.registry.YoRegistry;
+import us.ihmc.yoVariables.variable.YoBoolean;
+
+import static us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF.OdometryIndexHelper.*;
+import static us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF.OdometryIndexHelper.measurementAccelIndex;
+import static us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF.OdometryIndexHelper.measurementContactVelocityIndex;
+
+class MeasurementVariables
+{
+ public final YoVector3D relativePosition;
+ public final YoVector3D relativeOrientationError;
+ public final YoVector3D relativeLinearVelocityError;
+ public final YoVector3D contactVelocity;
+ public final YoVector3D accelMeasure;
+
+
+ public MeasurementVariables(String prefix, YoRegistry registry)
+ {
+ relativePosition = new YoVector3D(prefix + "RelativePosition", registry);
+ relativeOrientationError = new YoVector3D(prefix + "RelativeOrientationError", registry);
+ relativeLinearVelocityError = new YoVector3D(prefix + "RelativeLinearVelocityError", registry);
+ contactVelocity = new YoVector3D(prefix + "ContactVelocity", registry);
+ accelMeasure = new YoVector3D(prefix + "AccelMeasure", registry);
+ }
+
+ public void get(int start, DMatrixRMaj measurementToPack)
+ {
+ relativePosition.get(start + measurementRelativeTranslationIndex, measurementToPack);
+ relativeOrientationError.get(start + measurementRelativeOrientationErrorIndex, measurementToPack);
+ relativeLinearVelocityError.get(start + measurementRelativeVelocityIndex, measurementToPack);
+ contactVelocity.get(start + measurementContactVelocityIndex, measurementToPack);
+ accelMeasure.get(start + measurementAccelIndex, measurementToPack);
+ }
+}
diff --git a/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/ObservationModel.java b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/ObservationModel.java
new file mode 100644
index 000000000000..220f63eca37a
--- /dev/null
+++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/ObservationModel.java
@@ -0,0 +1,23 @@
+package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF;
+
+import org.ejml.data.DMatrixRMaj;
+
+import static us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF.OdometryIndexHelper.measurementAccelIndex;
+import static us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF.OdometryIndexHelper.measurementRelativeTranslationIndex;
+
+public class ObservationModel
+{
+ // State providers
+ private final SensedVariables footMeasurements;
+
+ public ObservationModel(SensedVariables footMeasurements)
+ {
+ this.footMeasurements = footMeasurements;
+ }
+
+ public void get(int startRow, DMatrixRMaj observation)
+ {
+ footMeasurements.positionMeasurement.get(startRow + measurementRelativeTranslationIndex, observation);
+ footMeasurements.accelMeasurement.get(startRow + measurementAccelIndex, observation);
+ }
+}
diff --git a/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryIndexHelper.java b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryIndexHelper.java
new file mode 100644
index 000000000000..ae1222f2e200
--- /dev/null
+++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryIndexHelper.java
@@ -0,0 +1,30 @@
+package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF;
+
+public class OdometryIndexHelper
+{
+ static final int stateSizePerLink = 16;
+ static final int stateTranslationIndex = 0;
+ static final int stateLinearVelocityIndex = 3;
+ static final int stateOrientationIndex = 6;
+ static final int stateAccelBiasIndex = 10;
+ static final int stateGyroBiasIndex = 13;
+
+ static final int errorSizePerLink = 15;
+ static final int errorTranslationIndex = 0;
+ static final int errorLinearVelocityIndex = 3;
+ static final int errorOrientationIndex = 6;
+ static final int errorAccelBiasIndex = 9;
+ static final int errorGyroBiasIndex = 12;
+
+ static final int measurementSizePerLink = 15;
+ static final int measurementRelativeTranslationIndex = 0;
+ static final int measurementRelativeOrientationErrorIndex = 3;
+ static final int measurementRelativeVelocityIndex = 6;
+ static final int measurementContactVelocityIndex = 9;
+ static final int measurementAccelIndex = 12;
+
+ public static int getFootPositionIndex(int footNumber)
+ {
+ return stateSizePerLink * (footNumber + 1) + stateTranslationIndex;
+ }
+}
diff --git a/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryKalmanFilter.java b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryKalmanFilter.java
new file mode 100644
index 000000000000..7f6c91825574
--- /dev/null
+++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryKalmanFilter.java
@@ -0,0 +1,498 @@
+package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF;
+
+import org.ejml.data.DMatrixRMaj;
+import org.ejml.dense.row.CommonOps_DDRM;
+import us.ihmc.euclid.referenceFrame.FramePoint3D;
+import us.ihmc.euclid.referenceFrame.FramePose3D;
+import us.ihmc.euclid.referenceFrame.FrameVector3D;
+import us.ihmc.euclid.referenceFrame.ReferenceFrame;
+import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameVector3DBasics;
+import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly;
+import us.ihmc.euclid.referenceFrame.tools.EuclidFrameFactories;
+import us.ihmc.euclid.transform.interfaces.RigidBodyTransformReadOnly;
+import us.ihmc.euclid.tuple3D.Vector3D;
+import us.ihmc.euclid.tuple4D.Quaternion;
+import us.ihmc.matrixlib.MatrixTools;
+import us.ihmc.mecano.multiBodySystem.interfaces.FloatingJointBasics;
+import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
+import us.ihmc.mecano.spatial.Twist;
+import us.ihmc.parameterEstimation.ExtendedKalmanFilter;
+import us.ihmc.sensorProcessing.stateEstimation.IMUSensorReadOnly;
+import us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.IMUBiasProvider;
+import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D;
+import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePose3D;
+import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameQuaternion;
+import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D;
+import us.ihmc.yoVariables.providers.BooleanProvider;
+import us.ihmc.yoVariables.registry.YoRegistry;
+import us.ihmc.yoVariables.variable.YoDouble;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import static us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF.OdometryIndexHelper.*;
+
+/**
+ * The process model in this class is defined as
+ */
+public class OdometryKalmanFilter extends ExtendedKalmanFilter
+{
+ static final boolean includeBias = true;
+ static final boolean usePredictedStateInJacobian = true;
+
+ // Constants and providers
+ private static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
+
+ private final YoRegistry registry = new YoRegistry(getClass().getSimpleName());
+
+ private final IMUSensorReadOnly baseIMU;
+ private final List feetIMUs;
+ private final IMUBiasProvider imuBiasProvider;
+
+ private final FrameVector3DReadOnly gravityVector;
+
+ private final OdometryKalmanFilterParameters parameters = new OdometryKalmanFilterParameters(registry);
+
+ private final YoDouble contactThreshold = new YoDouble("contactVarianceThreshold", registry);
+
+ // Internal variables for state holding
+ private final YoFrameVector3D yoBaseAngularVelocityMeasurement;
+
+ private final YoFrameVector3D yoRootJointAngularVelocityMeasurementInWorld;
+ private final YoFrameVector3D yoRootJointAngularVelocityMeasurement;
+
+ // Outputs
+ private final DMatrixRMaj predictedState;
+ private final DMatrixRMaj predictedMeasurement;
+ private final DMatrixRMaj observationVector;
+
+ private final DMatrixRMaj AMatrix;
+ private final DMatrixRMaj CMatrix;
+
+ private final SensedVariables baseSensing;
+ private final List feetSensing = new ArrayList<>();
+
+ private final StateVariables baseProcessState;
+ private final StateVariables basePredictedState;
+ private final StateCorrectionVariables baseStateCorrection;
+ private final List feetProcessState = new ArrayList<>();
+ private final List feetPredictedState = new ArrayList<>();
+ private final List feetStateCorrections = new ArrayList<>();
+ private final List feetMeasurementResiduals = new ArrayList<>();
+ private final List feetMeasurements = new ArrayList<>();
+
+ private final List processModels = new ArrayList<>();
+ private final List measurementModels = new ArrayList<>();
+ private final List observationModels = new ArrayList<>();
+
+ private final YoFrameQuaternion filteredBaseOrientation;
+ private final YoFramePoint3D filteredBaseTranslation;
+ private final YoFrameVector3D filteredBaseLinearVelocity;
+
+ private final YoFramePose3D estimatedRootJointPose;
+ private final YoFrameVector3D estimatedRootJointLinearVelocity;
+ private final YoFrameVector3D estimatedRootJointAngularVelocity;
+
+ private final RigidBodyTransformReadOnly transformToRootJoint;
+
+ // Temporary variables
+ private final DMatrixRMaj footCovariance = new DMatrixRMaj(3, 3);
+ private final DMatrixRMaj processCovariance;
+ private final DMatrixRMaj measurementCovariance;
+
+ private static final DMatrixRMaj eye3x3 = CommonOps_DDRM.identity(3);
+
+ public OdometryKalmanFilter(IMUSensorReadOnly baseIMU,
+ List feetIMUs,
+ IMUBiasProvider baseImuBiasProvider,
+ BooleanProvider cancelGravityFromAccelerationMeasurement,
+ double estimatorDT,
+ double gravitationalAcceleration,
+ YoRegistry parentRegistry)
+ {
+ super(stateSizePerLink * (1 + feetIMUs.size()), errorSizePerLink * (1 + feetIMUs.size()) , measurementSizePerLink * feetIMUs.size());
+
+ // Initialize covariance
+ processCovariance = new DMatrixRMaj(getErrorSize(), getErrorSize());
+ measurementCovariance = new DMatrixRMaj(getMeasurementSize(), getMeasurementSize());
+ MatrixTools.setDiagonal(processCovariance, 1e-2);
+ MatrixTools.setDiagonal(measurementCovariance, 1e-3);
+
+ contactThreshold.set(0.5);
+
+ this.baseIMU = baseIMU;
+ this.feetIMUs = feetIMUs;
+ this.imuBiasProvider = baseImuBiasProvider;
+
+ predictedState = new DMatrixRMaj(getStateSize(), 1);
+ predictedMeasurement = new DMatrixRMaj(getMeasurementSize(), 1);
+ AMatrix = new DMatrixRMaj(getErrorSize(), getErrorSize());
+ CMatrix = new DMatrixRMaj(getMeasurementSize(), getErrorSize());
+ observationVector = new DMatrixRMaj(getMeasurementSize(), 1);
+
+ gravityVector = EuclidFrameFactories.newLinkedFrameVector3DReadOnly(() -> worldFrame, new Vector3D(0, 0, Math.abs(gravitationalAcceleration)));
+
+ baseSensing = new SensedVariables("base", baseIMU, baseIMU, imuBiasProvider, registry);
+ baseProcessState = new StateVariables("baseProcessState", baseIMU.getMeasurementFrame(), registry);
+ basePredictedState = new StateVariables("basePredicted", baseIMU.getMeasurementFrame(), registry);
+ baseStateCorrection = new StateCorrectionVariables("baseStateCorrection", baseIMU.getMeasurementFrame(), registry);
+ processModels.add(new ProcessModel(baseProcessState,
+ basePredictedState,
+ cancelGravityFromAccelerationMeasurement,
+ gravityVector,
+ estimatorDT));
+ for (int i = 0; i < feetIMUs.size(); i++)
+ {
+ SensedVariables footSensing = new SensedVariables("foot" + i, baseIMU, feetIMUs.get(i), imuBiasProvider, registry);
+ StateVariables footProcessState = new StateVariables("foot" + i + "ProcessState", feetIMUs.get(i).getMeasurementFrame(), registry);
+ StateVariables footPredictedState = new StateVariables("foot" + i + "PredictedState", feetIMUs.get(i).getMeasurementFrame(), registry);
+ StateCorrectionVariables footStateCorrection = new StateCorrectionVariables("foot" + i + "StateCorrection", feetIMUs.get(i).getMeasurementFrame(), registry);
+ MeasurementVariables footMeasurement = new MeasurementVariables("foot" + i + "Predicted", registry);
+ MeasurementResidualVariables footMeasurementResidual = new MeasurementResidualVariables("foot" + i + "Residual", registry);
+ feetSensing.add(footSensing);
+ feetMeasurements.add(footMeasurement);
+ feetProcessState.add(footProcessState);
+ feetPredictedState.add(footPredictedState);
+ feetStateCorrections.add(footStateCorrection);
+ feetMeasurementResiduals.add(footMeasurementResidual);
+
+ observationModels.add(new ObservationModel(footSensing));
+ processModels.add(new ProcessModel(footProcessState,
+ footPredictedState,
+ cancelGravityFromAccelerationMeasurement,
+ gravityVector,
+ estimatorDT));
+ measurementModels.add(new MeasurementModel("foot" + i,
+ basePredictedState,
+ footPredictedState,
+ footSensing,
+ footMeasurement,
+ gravityVector,
+ contactThreshold,
+ registry));
+ }
+
+ filteredBaseOrientation = new YoFrameQuaternion("filteredBaseOrientation", worldFrame, registry);
+ filteredBaseTranslation = new YoFramePoint3D("filteredBaseTranslation", worldFrame, registry);
+ filteredBaseLinearVelocity = new YoFrameVector3D("filteredBaseLinearVelocity", worldFrame, registry);
+
+ estimatedRootJointPose = new YoFramePose3D("estimatedRootJointPose", worldFrame, registry);
+ estimatedRootJointLinearVelocity = new YoFrameVector3D("estimatedRootJointLinearVelocity", worldFrame, registry);
+ estimatedRootJointAngularVelocity = new YoFrameVector3D("estimatedRootJointAngularVelocity", worldFrame, registry);
+
+ transformToRootJoint = baseIMU.getMeasurementFrame().getTransformToDesiredFrame(baseIMU.getMeasurementLink().getParentJoint().getFrameAfterJoint());
+
+ yoBaseAngularVelocityMeasurement = new YoFrameVector3D("baseImuLinearAccelerationInWorld", baseIMU.getMeasurementFrame(), registry);
+ yoRootJointAngularVelocityMeasurement = new YoFrameVector3D("rootJointImuLinearAcceleration", baseIMU.getMeasurementFrame(), registry);
+ yoRootJointAngularVelocityMeasurementInWorld = new YoFrameVector3D("rootJointImuLinearAccelerationInWorld", worldFrame, registry);
+
+ parentRegistry.addChild(registry);
+ }
+
+ private void initialize()
+ {
+ baseProcessState.initialize();
+ for (int i = 0; i < feetProcessState.size(); i++)
+ feetProcessState.get(i).initialize();
+
+ // update the predicted state from the yo variablized state
+ baseProcessState.get(stateTranslationIndex, state);
+ for (int i = 0; i < feetIMUs.size(); i++)
+ {
+ feetProcessState.get(i).get(OdometryIndexHelper.getFootPositionIndex(i), state);
+ }
+
+ initializeState(state);
+ }
+
+ private boolean initialized = false;
+
+ public void compute()
+ {
+ updateCovariance();
+
+ baseSensing.update();
+ for (int i = 0; i < feetSensing.size(); i++)
+ feetSensing.get(i).update();
+
+ for (int i = 0; i < observationModels.size(); i++)
+ {
+ observationModels.get(i).get(i * measurementSizePerLink, observationVector);
+ }
+
+ if (!initialized)
+ {
+ initialize();
+ initialized = true;
+ }
+
+ updateRootJointTwistAngularPart(); // TODO maybe do this after we update everything based on the estimate?
+
+ DMatrixRMaj stateEstimate = calculateEstimate(observationVector);
+
+ filteredBaseTranslation.set(stateEstimate);
+ filteredBaseLinearVelocity.set(3, stateEstimate);
+ filteredBaseOrientation.set(6, stateEstimate);
+
+ // Transform the pose of the IMU in the world frame to the pose of the root joint in the world frame
+ tempPose.set(filteredBaseOrientation, filteredBaseTranslation);
+ tempPose.appendTransform(transformToRootJoint);
+ estimatedRootJointPose.set(tempPose);
+ estimatedRootJointAngularVelocity.set(yoRootJointAngularVelocityMeasurementInWorld);
+
+ updateRootJointTwistLinearPart(filteredBaseLinearVelocity, estimatedRootJointAngularVelocity, estimatedRootJointLinearVelocity);
+ }
+
+ private void updateCovariance()
+ {
+ OdometryTools.setDiagonals(errorTranslationIndex, 3, parameters.baseProcessTranslationNoise.getValue(), processCovariance);
+ OdometryTools.setDiagonals(errorLinearVelocityIndex, 2, parameters.baseProcessVelocityXYNoise.getValue(), processCovariance);
+ OdometryTools.setDiagonals(errorLinearVelocityIndex + 2, 1, parameters.baseProcessVelocityZNoise.getValue(), processCovariance);
+ OdometryTools.setDiagonals(errorOrientationIndex, 3, parameters.baseProcessOrientationNoise.getValue(), processCovariance);
+ OdometryTools.setDiagonals(errorAccelBiasIndex, 3, parameters.baseProcessAccelBiasNoise.getValue(), processCovariance);
+ OdometryTools.setDiagonals(errorGyroBiasIndex, 3, parameters.baseProcessGyroBiasNoise.getValue(), processCovariance);
+
+ for (int i = 0; i < feetIMUs.size(); i++)
+ {
+ int index = (i + 1) * errorSizePerLink;
+ OdometryTools.setDiagonals(index + errorTranslationIndex, 3, parameters.footProcessTranslationNoise.getValue(), processCovariance);
+ OdometryTools.setDiagonals(index + errorLinearVelocityIndex, 3, parameters.footProcessVelocityNoise.getValue(), processCovariance);
+ OdometryTools.setDiagonals(index + errorOrientationIndex, 3, parameters.footProcessOrientationNoise.getValue(), processCovariance);
+ OdometryTools.setDiagonals(index + errorAccelBiasIndex, 3, parameters.footProcessAccelBiasNoise.getValue(), processCovariance);
+ OdometryTools.setDiagonals(index + errorGyroBiasIndex, 3, parameters.footProcessGyroBiasNoise.getValue(), processCovariance);
+ }
+
+ for (int i = 0; i < feetIMUs.size(); i++)
+ {
+ int index = i * measurementSizePerLink;
+ OdometryTools.setDiagonals(index + measurementRelativeTranslationIndex, 3, parameters.measurementIKPositionNoise.getValue(), measurementCovariance);
+ OdometryTools.setDiagonals(index + measurementRelativeOrientationErrorIndex, 3, parameters.measurementIKOrientationNoise.getValue(), measurementCovariance);
+ OdometryTools.setDiagonals(index + measurementRelativeVelocityIndex, 3, parameters.measurementIKVelocityNoise.getValue(), measurementCovariance);
+ OdometryTools.setDiagonals(index + measurementContactVelocityIndex, 3, parameters.measurementContactVelocityNoise.getValue(), measurementCovariance);
+ OdometryTools.setDiagonals(index + measurementAccelIndex, 3, parameters.measurementContactAccelNoise.getValue(), measurementCovariance);
+ }
+
+ setProcessCovariance(processCovariance);
+ setMeasurementCovariance(measurementCovariance);
+ }
+
+ private final FramePose3D tempPose = new FramePose3D();
+
+ private final Vector3D angularVelocityMeasurement = new Vector3D();
+
+ /** Angular velocity of the measurement link, with respect to world. */
+ private final FrameVector3D angularVelocityMeasurementLinkRelativeToWorld = new FrameVector3D();
+
+ /** Angular velocity of the estimation link, with respect to the measurement link. */
+ private final FrameVector3D angularVelocityRootJointFrameRelativeToMeasurementLink = new FrameVector3D();
+
+ /** Twist of the estimation link, with respect to the measurement link. */
+ private final Twist twistRootJointFrameRelativeToMeasurementLink = new Twist();
+
+ private void updateRootJointTwistAngularPart()
+ {
+ RigidBodyBasics measurementLink = baseIMU.getMeasurementLink();
+ FloatingJointBasics rootJoint = (FloatingJointBasics) baseIMU.getMeasurementLink().getParentJoint();
+ ReferenceFrame rootJointFrame = rootJoint.getFrameAfterJoint();
+ // T_{rootBody}^{rootBody, measurementLink}
+ rootJoint.getSuccessor().getBodyFixedFrame().getTwistRelativeToOther(measurementLink.getBodyFixedFrame(), twistRootJointFrameRelativeToMeasurementLink);
+ // T_{rootBody}^{rootJointFrame, measurementLink}
+ twistRootJointFrameRelativeToMeasurementLink.changeFrame(rootJointFrame);
+ // T_{rootJointFrame}^{rootJointFrame, measurementLink}
+ twistRootJointFrameRelativeToMeasurementLink.setBodyFrame(rootJointFrame);
+
+ // omega_{rootJointFrame}^{rootJointFrame, measurementLink}
+ angularVelocityRootJointFrameRelativeToMeasurementLink.setIncludingFrame(twistRootJointFrameRelativeToMeasurementLink.getAngularPart());
+
+ // omega_{measurementLink}^{measurementFrame, world}
+ angularVelocityMeasurement.set(baseIMU.getAngularVelocityMeasurement());
+ if (imuBiasProvider != null)
+ {
+ FrameVector3DReadOnly angularVelocityBiasInIMUFrame = imuBiasProvider.getAngularVelocityBiasInIMUFrame(baseIMU);
+ if (angularVelocityBiasInIMUFrame != null)
+ angularVelocityMeasurement.sub(angularVelocityBiasInIMUFrame);
+ }
+ angularVelocityMeasurementLinkRelativeToWorld.setIncludingFrame(baseIMU.getMeasurementFrame(), angularVelocityMeasurement);
+
+ // omega_{measurementLink}^{rootJointFrame, world}
+ angularVelocityMeasurementLinkRelativeToWorld.changeFrame(rootJointFrame);
+
+ // omega_{rootJointFrame}^{rootJointFrame, world} = omega_{rootJointFrame}^{rootJointFrame, measurementLink} + omega_{measurementLink}^{rootJointFrame, world}
+ rootJoint.getJointTwist().getAngularPart().add(angularVelocityRootJointFrameRelativeToMeasurementLink, angularVelocityMeasurementLinkRelativeToWorld);
+ rootJoint.updateFrame();
+
+ yoRootJointAngularVelocityMeasurement.setMatchingFrame(rootJoint.getJointTwist().getAngularPart());
+ yoRootJointAngularVelocityMeasurementInWorld.setMatchingFrame(rootJoint.getJointTwist().getAngularPart());
+ yoBaseAngularVelocityMeasurement.setMatchingFrame(angularVelocityMeasurementLinkRelativeToWorld);
+ }
+
+ private final FramePoint3D imuToRoot = new FramePoint3D();
+
+ private final FrameVector3D addedAngular = new FrameVector3D();
+ private final FrameVector3D momentArm = new FrameVector3D();
+ private void updateRootJointTwistLinearPart(FrameVector3DReadOnly imuLinearVelocity,
+ FrameVector3DReadOnly imuAngularVelocity,
+ FixedFrameVector3DBasics rootLinearVelocityToPack)
+ {
+ FloatingJointBasics rootJoint = (FloatingJointBasics) baseIMU.getMeasurementLink().getParentJoint();
+ ReferenceFrame rootJointFrame = rootJoint.getFrameAfterJoint();
+
+ imuToRoot.setToZero(baseIMU.getMeasurementFrame());
+ imuToRoot.changeFrame(rootJointFrame);
+
+ momentArm.setIncludingFrame(imuToRoot);
+ momentArm.changeFrame(worldFrame);
+
+ addedAngular.cross(momentArm, imuAngularVelocity);
+
+ rootLinearVelocityToPack.set(imuLinearVelocity);
+ rootLinearVelocityToPack.add(addedAngular);
+ }
+
+ @Override
+ protected void postSolveHook()
+ {
+ // Update some yo variables
+ baseStateCorrection.set(0, stateCorrection);
+ for (int i = 0; i < feetIMUs.size(); i++)
+ {
+ feetStateCorrections.get(i).set((i + 1) * errorSizePerLink, stateCorrection);
+ feetMeasurementResiduals.get(i).set(i * measurementSizePerLink, measurementResidual);
+ }
+ }
+
+ @Override
+ protected void applyStateCorrection(DMatrixRMaj state, DMatrixRMaj correction, DMatrixRMaj correctedState)
+ {
+ correctedState.set(state);
+ for (int i = 0; i < feetIMUs.size() + 1; i++)
+ {
+ int destStart = stateSizePerLink * i;
+ int sourceStart = errorSizePerLink * i;
+ // Update the translation
+ MatrixTools.addMatrixBlock(correctedState, destStart + stateTranslationIndex, 0, correction, sourceStart + errorTranslationIndex, 0, 3, 1, 1.0);
+ // Update the linear velocity
+ MatrixTools.addMatrixBlock(correctedState, destStart + stateLinearVelocityIndex, 0, correction, sourceStart + errorLinearVelocityIndex, 0, 3, 1, 1.0);
+ // Update the orientation
+ // FIXME garbage
+ Vector3D rotationVector = new Vector3D();
+ rotationVector.set(sourceStart + errorOrientationIndex, correction);
+ Quaternion correctionRotation = new Quaternion(rotationVector);
+ Quaternion rotation = new Quaternion();
+ rotation.set(destStart + stateOrientationIndex, state);
+ rotation.multiply(correctionRotation);
+ rotation.get(destStart + stateOrientationIndex, correctedState);
+ // Update the accel bias
+ MatrixTools.addMatrixBlock(correctedState, destStart + stateAccelBiasIndex, 0, correction, sourceStart + errorAccelBiasIndex, 0, 3, 1, 1.0);
+ // Update the gyro bias
+ MatrixTools.addMatrixBlock(correctedState, destStart + stateGyroBiasIndex, 0, correction, sourceStart + errorGyroBiasIndex, 0, 3, 1, 1.0);
+ }
+ }
+
+ @Override
+ public DMatrixRMaj processModel(DMatrixRMaj state)
+ {
+ predictedState.zero();
+
+ // update the yo variables representing the state
+ baseProcessState.set(stateTranslationIndex, state, baseSensing);
+ for (int i = 0; i < feetIMUs.size(); i++)
+ {
+ feetProcessState.get(i).set(OdometryIndexHelper.getFootPositionIndex(i), state, feetSensing.get(i));
+ }
+
+ // do the prediction
+ for (int i = 0; i < processModels.size(); i++)
+ processModels.get(i).update();
+
+ // update the predicted state from the yo variablized state
+ basePredictedState.get(stateTranslationIndex, predictedState);
+ for (int i = 0; i < feetIMUs.size(); i++)
+ {
+ feetPredictedState.get(i).get(OdometryIndexHelper.getFootPositionIndex(i), predictedState);
+ }
+
+ // return the predicted state
+ return predictedState;
+ }
+
+ @Override
+ public DMatrixRMaj measurementModel(DMatrixRMaj predictedState)
+ {
+ predictedMeasurement.zero();
+
+ // Note that this bypasses the use of predicted state vector, as the predicted state variables are computed in the {@link #processModel(DMatrixRMaj)} call
+ for (int i = 0; i < feetIMUs.size(); i++)
+ {
+ int row = errorSizePerLink * (i + 1) + measurementContactVelocityIndex;
+ MatrixTools.setMatrixBlock(footCovariance, 0, 0, getCovariance(), row, row, 3, 3, 1.0);
+
+ measurementModels.get(i).update(footCovariance);
+ feetMeasurements.get(i).get(i * measurementSizePerLink, predictedMeasurement);
+ }
+
+ // return the predicted measurement
+ return predictedMeasurement;
+ }
+
+ @Override
+ protected DMatrixRMaj linearizeProcessModel(DMatrixRMaj previousState)
+ {
+ // make sure to call zero, as the compute process model stuff has a bunch of add operators.
+ AMatrix.zero();
+ int offset = 0;
+
+ for (int i = 0; i < feetIMUs.size() + 1; i++)
+ {
+ processModels.get(i).computeProcessJacobian(offset, AMatrix);
+ offset += errorSizePerLink;
+ }
+
+ return AMatrix;
+ }
+
+ @Override
+ protected DMatrixRMaj linearizeMeasurementModel(DMatrixRMaj predictedState)
+ {
+ // make sure to call zero, as the compute process model stuff has a bunch of add operators.
+ CMatrix.zero();
+ int rowOffset = 0;
+ int colOffset = errorSizePerLink;
+ for (int i = 0; i < feetIMUs.size(); i++)
+ {
+ MeasurementModel measurementModel = measurementModels.get(i);
+ measurementModel.computeBaseMeasurementJacobian(rowOffset, CMatrix);
+ measurementModel.computeFootMeasurementJacobian(rowOffset, colOffset, CMatrix);
+ rowOffset += measurementSizePerLink;
+ colOffset += errorSizePerLink;
+ }
+
+ return CMatrix;
+ }
+
+ @Override
+ protected void maskResidualCovarianceForOutlierRejection()
+ {
+ // FIXME garbage
+ DMatrixRMaj mask = CommonOps_DDRM.identity(getMeasurementSize(), getMeasurementSize());
+ DMatrixRMaj maskTransposeS = new DMatrixRMaj(0, 0);
+
+ for (int i = feetIMUs.size() - 1; i >= 0; i--)
+ {
+ if (!measurementModels.get(i).getIsFootInContact())
+ {
+ int index = i * measurementSizePerLink;
+ // remove the rows of the measurement accel, since it's larger
+ CommonOps_DDRM.removeColumns(mask, index + measurementAccelIndex, index + measurementAccelIndex + 2);
+ CommonOps_DDRM.removeColumns(mask, index + measurementContactVelocityIndex, index + measurementContactVelocityIndex + 2);
+ }
+ }
+
+ CommonOps_DDRM.multTransA(mask, measurementResidual, maskedMeasurementResidual);
+ CommonOps_DDRM.multTransA(mask, measurementJacobian, maskedMeasurementJacobian);
+ CommonOps_DDRM.multTransA(mask, residualCovariance, maskTransposeS);
+ CommonOps_DDRM.mult(maskTransposeS, mask, maskedResidualCovariance);
+ CommonOps_DDRM.multTransA(mask, measurementCovariance, maskTransposeS);
+ CommonOps_DDRM.mult(maskTransposeS, mask, maskedMeasurementCovariance);
+ }
+}
diff --git a/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryKalmanFilterParameters.java b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryKalmanFilterParameters.java
new file mode 100644
index 000000000000..2e959e3a35d0
--- /dev/null
+++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryKalmanFilterParameters.java
@@ -0,0 +1,53 @@
+package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF;
+
+import us.ihmc.yoVariables.registry.YoRegistry;
+import us.ihmc.yoVariables.variable.YoDouble;
+
+public class OdometryKalmanFilterParameters
+{
+ private final YoRegistry registry = new YoRegistry(getClass().getSimpleName());
+
+ public final YoDouble baseProcessTranslationNoise = new YoDouble("baseProcessTranslationNoise", registry);
+ public final YoDouble baseProcessVelocityXYNoise = new YoDouble("baseProcessVelocityXYNoise", registry);
+ public final YoDouble baseProcessVelocityZNoise = new YoDouble("baseProcessVelocityZNoise", registry);
+ public final YoDouble baseProcessOrientationNoise = new YoDouble("baseProcessOrientationNoise", registry);
+ public final YoDouble baseProcessGyroBiasNoise = new YoDouble("baseProcessGyroBiasNoise", registry);
+ public final YoDouble baseProcessAccelBiasNoise = new YoDouble("baseProcessAccelBiasNoise", registry);
+
+ public final YoDouble footProcessTranslationNoise = new YoDouble("footProcessTranslationNoise", registry);
+ public final YoDouble footProcessVelocityNoise = new YoDouble("footProcessVelocityNoise", registry);
+ public final YoDouble footProcessOrientationNoise = new YoDouble("footProcessOrientationNoise", registry);
+ public final YoDouble footProcessAccelBiasNoise = new YoDouble("footProcessAccelBiasNoise", registry);
+ public final YoDouble footProcessGyroBiasNoise = new YoDouble("footProcessGyroBiasNoise", registry);
+
+
+ public final YoDouble measurementIKPositionNoise = new YoDouble("measurementIKPositionNoise", registry);
+ public final YoDouble measurementIKVelocityNoise = new YoDouble("measurementIKVelocityNoise", registry);
+ public final YoDouble measurementIKOrientationNoise = new YoDouble("measurementIKOrientationNoise", registry);
+ public final YoDouble measurementContactVelocityNoise = new YoDouble("measurementContactVelocityNoise", registry);
+ public final YoDouble measurementContactAccelNoise = new YoDouble("measurementContactAccelNoise", registry);
+
+ public OdometryKalmanFilterParameters(YoRegistry parentRegistry)
+ {
+ baseProcessTranslationNoise.set(0.0005);
+ baseProcessVelocityXYNoise.set(0.005);
+ baseProcessVelocityZNoise.set(0.005);
+ baseProcessOrientationNoise.set(1e-7);
+ baseProcessGyroBiasNoise.set(1e-5);
+ baseProcessAccelBiasNoise.set(1e-4);
+
+ footProcessTranslationNoise.set(1e-4);
+ footProcessVelocityNoise.set(5.0);
+ footProcessOrientationNoise.set(1e-5); // made up.
+ footProcessGyroBiasNoise.set(1e-5); // made up
+ footProcessAccelBiasNoise.set(1e-4);
+
+ measurementIKPositionNoise.set(0.001);
+ measurementIKOrientationNoise.set(1e-8);
+ measurementIKVelocityNoise.set(0.05);
+ measurementContactVelocityNoise.set(0.01);
+ measurementContactAccelNoise.set(1.0);
+
+ parentRegistry.addChild(registry);
+ }
+}
diff --git a/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryTools.java b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryTools.java
new file mode 100644
index 000000000000..93c8f3fa3f14
--- /dev/null
+++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryTools.java
@@ -0,0 +1,278 @@
+package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF;
+
+import org.ejml.data.DMatrixRMaj;
+import org.ejml.dense.row.CommonOps_DDRM;
+import us.ihmc.euclid.tools.EuclidCoreTools;
+import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly;
+import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics;
+import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly;
+import us.ihmc.euclid.tuple4D.interfaces.QuaternionBasics;
+import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly;
+import us.ihmc.matrixlib.MatrixTools;
+
+public class OdometryTools
+{
+ private static final DMatrixRMaj eye3x3 = CommonOps_DDRM.identity(3);
+
+ static void toRotationMatrixInverse(double qx, double qy, double qz, double qs, DMatrixRMaj rotationMatrix)
+ {
+ double qx2 = qx * qx;
+ double qy2 = qy * qy;
+ double qz2 = qz * qz;
+ double qs2 = qs * qs;
+
+ rotationMatrix.set(0, 0, qx2 - qy2 - qz2 + qs2);
+ rotationMatrix.set(1, 1, -qx2 + qy2 - qz2 + qs2);
+ rotationMatrix.set(2, 2, -qx2 - qy2 + qz2 + qs2);
+
+ double xy = qx * qy;
+ double xz = qx * qz;
+ double yz = qy * qz;
+ double xw = qx * qs;
+ double yw = qy * qs;
+ double zw = qz * qs;
+
+ rotationMatrix.set(1, 0, 2.0 * (xy - zw));
+ rotationMatrix.set(0, 1, 2.0 * (xy + zw));
+
+ rotationMatrix.set(2, 0, 2.0 * (xz + yw));
+ rotationMatrix.set(0, 2, 2.0 * (xz - yw));
+
+ rotationMatrix.set(2, 1, 2.0 * (yz - xw));
+ rotationMatrix.set(1, 2, 2.0 * (yz + xw));
+ }
+
+ static void toRotationMatrixInverse(QuaternionReadOnly quaternion, DMatrixRMaj rotationMatrix)
+ {
+ toRotationMatrixInverse(quaternion.getX(), quaternion.getY(), quaternion.getZ(), quaternion.getS(), rotationMatrix);
+ }
+
+ static void toSkewSymmetricMatrix(double x, double y, double z, DMatrixRMaj matrixToPack)
+ {
+ matrixToPack.zero();
+ matrixToPack.set(0, 1, -z);
+ matrixToPack.set(0, 2, y);
+ matrixToPack.set(1, 0, z);
+ matrixToPack.set(1, 2, -x);
+ matrixToPack.set(2, 0, -y);
+ matrixToPack.set(2, 1, x);
+ }
+
+ static void toSkewSymmetricMatrix(double x, double y, double z, int row, int col, DMatrixRMaj matrixToPack)
+ {
+ matrixToPack.zero();
+ matrixToPack.set(row, col + 1, -z);
+ matrixToPack.set(row, col + 2, y);
+ matrixToPack.set(row + 1, col, z);
+ matrixToPack.set(row + 1, col + 2, -x);
+ matrixToPack.set(row + 2, col, -y);
+ matrixToPack.set(row + 2, col + 1, x);
+ }
+
+ static void toSkewSymmetricMatrix(Tuple3DReadOnly vector, DMatrixRMaj matrixToPack)
+ {
+ toSkewSymmetricMatrix(vector.getX(), vector.getY(), vector.getZ(), matrixToPack);
+ }
+
+ static void toRotationMatrix(double qx, double qy, double qz, double qs, DMatrixRMaj rotationMatrix)
+ {
+ double qx2 = qx * qx;
+ double qy2 = qy * qy;
+ double qz2 = qz * qz;
+ double qs2 = qs * qs;
+
+ rotationMatrix.set(0, 0, qx2 - qy2 - qz2 + qs2);
+ rotationMatrix.set(1, 1, -qx2 + qy2 - qz2 + qs2);
+ rotationMatrix.set(2, 2, -qx2 - qy2 + qz2 + qs2);
+
+ double xy = qx * qy;
+ double xz = qx * qz;
+ double yz = qy * qz;
+ double xw = qx * qs;
+ double yw = qy * qs;
+ double zw = qz * qs;
+
+ rotationMatrix.set(0, 1, 2.0 * (xy - zw));
+ rotationMatrix.set(1, 0, 2.0 * (xy + zw));
+
+ rotationMatrix.set(0, 2, 2.0 * (xz + yw));
+ rotationMatrix.set(2, 0, 2.0 * (xz - yw));
+
+ rotationMatrix.set(1, 2, 2.0 * (yz - xw));
+ rotationMatrix.set(2, 1, 2.0 * (yz + xw));
+ }
+
+ static void toRotationMatrix(QuaternionReadOnly quaternion, DMatrixRMaj rotationMatrix)
+ {
+ toRotationMatrix(quaternion.getX(), quaternion.getY(), quaternion.getZ(), quaternion.getS(), rotationMatrix);
+ }
+
+ static void setDiagonals(int start, int elements, double value, DMatrixRMaj matrix)
+ {
+ for (int i = start; i < start + elements; i++)
+ matrix.set(i, i, value);
+ }
+
+ public static void toQuaternionFromRotationVectorSmallAngle(Vector3DReadOnly rotation, QuaternionBasics quaternionToPack)
+ {
+ double magnitude = rotation.norm();
+ double s = magnitude * 0.5;
+ double qs = 1.0;
+ double qx = s * rotation.getX() / magnitude;
+ double qy = s * rotation.getY() / magnitude;
+ double qz = s * rotation.getZ() / magnitude;
+ quaternionToPack.set(qx, qy, qz, qs);
+ }
+
+ public static void logMap(QuaternionReadOnly quaternion, Vector3DBasics rotationToPack)
+ {
+ // TODO is this just a transform to the axis-angle representation?
+ double norm = EuclidCoreTools.norm(quaternion.getX(), quaternion.getY(), quaternion.getZ());
+ double scale = 2.0 * Math.atan2(norm, quaternion.getS()) / norm;
+ rotationToPack.set(quaternion.getX(), quaternion.getY(), quaternion.getZ());
+ rotationToPack.scale(scale);
+ }
+
+ public static void logMapSmallAngle(QuaternionReadOnly quaternion, Vector3DBasics rotationToPack)
+ {
+ rotationToPack.set(quaternion.getX(), quaternion.getY(), quaternion.getZ());
+ rotationToPack.scale(2.0);
+ }
+
+ static DMatrixRMaj lOperator(DMatrixRMaj quaternion)
+ {
+ return lOperator(quaternion.get(0), quaternion.get(1), quaternion.get(2), quaternion.get(3));
+ }
+
+ static DMatrixRMaj lOperator(QuaternionReadOnly quaternion)
+ {
+ return lOperator(quaternion.getS(), quaternion.getX(), quaternion.getY(), quaternion.getZ());
+ }
+
+ static DMatrixRMaj lOperator(double qs, double qx, double qy, double qz)
+ {
+ DMatrixRMaj lOperator = new DMatrixRMaj(4, 4);
+ lOperator(qs, qx, qy, qz, lOperator);
+ return lOperator;
+ }
+
+ static void lOperator(QuaternionReadOnly quaternion, DMatrixRMaj lOperatorToPack)
+ {
+ lOperator(quaternion.getS(), quaternion.getX(), quaternion.getY(), quaternion.getZ(), lOperatorToPack);
+ }
+
+ static void lOperator(double qs, double qx, double qy, double qz, DMatrixRMaj lOperatorToPack)
+ {
+ toSkewSymmetricMatrix(qx, qy, qz, 1, 1, lOperatorToPack);
+ for (int i = 1; i < 4; i++)
+ lOperatorToPack.add(i, i, qs);
+ lOperatorToPack.set(0, 0, qs);
+ lOperatorToPack.set(0, 1, -qx);
+ lOperatorToPack.set(0, 2, -qy);
+ lOperatorToPack.set(0, 3, -qz);
+ lOperatorToPack.set(1, 0, qx);
+ lOperatorToPack.set(2, 0, qy);
+ lOperatorToPack.set(3, 0, qz);
+ }
+
+ static DMatrixRMaj rOperator(DMatrixRMaj quaternion)
+ {
+ return rOperator(quaternion.get(0), quaternion.get(1), quaternion.get(2), quaternion.get(3));
+ }
+
+ static DMatrixRMaj rOperator(QuaternionReadOnly quaternion)
+ {
+ return rOperator(quaternion.getS(), quaternion.getX(), quaternion.getY(), quaternion.getZ());
+ }
+
+ static DMatrixRMaj rOperator(double qs, double qx, double qy, double qz)
+ {
+ DMatrixRMaj rOperator = new DMatrixRMaj(4, 4);
+ rOperator(qs, qx, qy, qz, rOperator);
+ return rOperator;
+ }
+
+ static void rOperator(QuaternionReadOnly quaternion, DMatrixRMaj lOperatorToPack)
+ {
+ rOperator(quaternion.getS(), quaternion.getX(), quaternion.getY(), quaternion.getZ(), lOperatorToPack);
+ }
+
+ static void rOperator(double qs, double qx, double qy, double qz, DMatrixRMaj rOperatorToPack)
+ {
+ toSkewSymmetricMatrix(-qx, -qy, -qz, 1, 1, rOperatorToPack);
+ for (int i = 1; i < 4; i++)
+ rOperatorToPack.add(i, i, qs);
+ rOperatorToPack.set(0, 0, qs);
+ rOperatorToPack.set(0, 1, -qx);
+ rOperatorToPack.set(0, 2, -qy);
+ rOperatorToPack.set(0, 3, -qz);
+ rOperatorToPack.set(1, 0, qx);
+ rOperatorToPack.set(2, 0, qy);
+ rOperatorToPack.set(3, 0, qz);
+ }
+
+ static void l3Operator(QuaternionReadOnly quaternion, DMatrixRMaj matrixToPack)
+ {
+ toSkewSymmetricMatrix(quaternion.getX(), quaternion.getY(), quaternion.getZ(), matrixToPack);
+ MatrixTools.addDiagonal(matrixToPack, quaternion.getS());
+ }
+
+ static void r3Operator(QuaternionReadOnly quaternion, DMatrixRMaj matrixToPack)
+ {
+ toSkewSymmetricMatrix(quaternion.getX(), quaternion.getY(), quaternion.getZ(), matrixToPack);
+ CommonOps_DDRM.scale(-1.0, matrixToPack);
+ MatrixTools.addDiagonal(matrixToPack, quaternion.getS());
+ }
+
+ static DMatrixRMaj exponentialMap(DMatrixRMaj vector)
+ {
+ return exponentialMap(vector.get(0), vector.get(1), vector.get(2));
+ }
+
+ static DMatrixRMaj exponentialMap(Vector3DReadOnly vector)
+ {
+ return exponentialMap(vector.getX(), vector.getY(), vector.getZ());
+ }
+
+ static DMatrixRMaj exponentialMap(double x, double y, double z)
+ {
+ DMatrixRMaj map = new DMatrixRMaj(4, 1);
+
+ double length = EuclidCoreTools.norm(x, y, z);
+
+ if (Math.abs(length) < 1e-5 )
+ {
+ map.set(0, 0, 1.0);
+ return map;
+ }
+ double s = Math.sin(length / 2.0);
+ map.set(0, 0, Math.cos(length / 2.0));
+ map.set(1, 0, s * x / length);
+ map.set(2, 0, s * y / length);
+ map.set(3, 0, s * z / length);
+
+ return map;
+ }
+
+ static DMatrixRMaj logMap(QuaternionReadOnly quaternion)
+ {
+ return logMap(quaternion.getS(), quaternion.getX(), quaternion.getY(), quaternion.getZ());
+ }
+
+ static DMatrixRMaj logMap(DMatrixRMaj quaternion)
+ {
+ return logMap(quaternion.get(0), quaternion.get(1), quaternion.get(2), quaternion.get(3));
+ }
+
+ static DMatrixRMaj logMap(double qs, double qx, double qy, double qz)
+ {
+ double length = EuclidCoreTools.norm(qx, qy, qz);
+ double multiplier = 2.0 * Math.atan2(length, qs) / length;
+ DMatrixRMaj ret = new DMatrixRMaj(3, 1);
+ ret.set(0, qx * multiplier);
+ ret.set(1, qy * multiplier);
+ ret.set(2, qz * multiplier);
+
+ return ret;
+ }
+}
diff --git a/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/ProcessModel.java b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/ProcessModel.java
new file mode 100644
index 000000000000..b08887592814
--- /dev/null
+++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/ProcessModel.java
@@ -0,0 +1,120 @@
+package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF;
+
+import org.ejml.data.DMatrixRMaj;
+import org.ejml.dense.row.CommonOps_DDRM;
+import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly;
+import us.ihmc.euclid.tools.QuaternionTools;
+import us.ihmc.euclid.tuple3D.Vector3D;
+import us.ihmc.euclid.tuple4D.Quaternion;
+import us.ihmc.log.LogTools;
+import us.ihmc.matrixlib.MatrixTools;
+import us.ihmc.yoVariables.providers.BooleanProvider;
+
+import static us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF.OdometryIndexHelper.*;
+
+class ProcessModel
+{
+ private static final DMatrixRMaj eye3x3 = CommonOps_DDRM.identity(3);
+ private final DMatrixRMaj deltaT;
+
+ private static final boolean usePredictedInJacobian = true;
+
+ // State providers
+ private final StateVariables currentState;
+ private final StateVariables predictedState;
+ private final BooleanProvider cancelGravityFromAccelerationMeasurement;
+ private final FrameVector3DReadOnly gravityVector;
+ private final double estimatorDt;
+
+ // Temp variables
+ private final Vector3D integratedVelocity = new Vector3D();
+ private final Vector3D unbiasedAcceleration = new Vector3D();
+ private final Quaternion integratedRotation = new Quaternion();
+ private final DMatrixRMaj rotationMatrix = new DMatrixRMaj(3, 3);
+ private final DMatrixRMaj skewMatrix = new DMatrixRMaj(3, 3);
+
+
+ public ProcessModel(StateVariables processState,
+ StateVariables predictedState,
+ BooleanProvider cancelGravityFromAccelerationMeasurement,
+ FrameVector3DReadOnly gravityVector,
+ double estimatorDt)
+ {
+ this.currentState = processState;
+ this.predictedState = predictedState;
+ this.cancelGravityFromAccelerationMeasurement = cancelGravityFromAccelerationMeasurement;
+ this.gravityVector = gravityVector;
+ this.estimatorDt = estimatorDt;
+
+ deltaT = new DMatrixRMaj(eye3x3);
+ CommonOps_DDRM.scale(estimatorDt, deltaT);
+ }
+
+ public void update()
+ {
+ // transform the acceleration to world
+ unbiasedAcceleration.set(currentState.unbiasedAccel);
+ currentState.orientation.transform(unbiasedAcceleration);
+ if (cancelGravityFromAccelerationMeasurement.getValue())
+ {
+ // remove gravity from acceleration
+ unbiasedAcceleration.sub(gravityVector);
+ }
+
+ // First order integration of the position state
+ predictedState.translation.scaleAdd(estimatorDt, currentState.linearVelocity, currentState.translation);
+
+ // Integration of the velocity state
+ predictedState.linearVelocity.scaleAdd(estimatorDt, unbiasedAcceleration, currentState.linearVelocity);
+
+ // First order integration of the angular state along the SO(3) manifold
+ integratedVelocity.setAndScale(estimatorDt, currentState.unbiasedGyro);
+ integratedRotation.setRotationVector(integratedVelocity);
+ QuaternionTools.multiply(currentState.orientation, integratedRotation, predictedState.orientation);
+
+ // Propagate the bias
+ predictedState.accelBias.set(currentState.accelBias);
+ predictedState.gyroBias.set(currentState.gyroBias);
+ }
+
+ /**
+ * This is equation 39
+ */
+ public void computeProcessJacobian(int offset, DMatrixRMaj jacobianToPack)
+ {
+ StateVariables stateVariables = OdometryKalmanFilter.usePredictedStateInJacobian ? predictedState : currentState;
+ // First row. Partial derivative with respect to the base position state
+ // p_k+1 = p_k + v_k deltaT
+ int rowStart = offset + errorTranslationIndex;
+ CommonOps_DDRM.insert(eye3x3, jacobianToPack, rowStart, offset + errorTranslationIndex);
+ CommonOps_DDRM.insert(deltaT, jacobianToPack, rowStart, offset + errorLinearVelocityIndex);
+
+ // Second row. Partial derivative with respect to the base velocity state
+ // v_k+1 = v_k + deltaT * (R * a - g)
+ rowStart = offset + errorLinearVelocityIndex;
+ CommonOps_DDRM.insert(eye3x3, jacobianToPack, rowStart, offset + errorLinearVelocityIndex);
+
+
+ OdometryTools.toRotationMatrix(stateVariables.orientation, rotationMatrix);
+ OdometryTools.toSkewSymmetricMatrix(stateVariables.unbiasedAccel, skewMatrix);
+ MatrixTools.multAddBlock(-estimatorDt, rotationMatrix, skewMatrix, jacobianToPack, rowStart, offset + errorOrientationIndex);
+
+ if (OdometryKalmanFilter.includeBias)
+ MatrixTools.setMatrixBlock(jacobianToPack, rowStart, offset + errorAccelBiasIndex, rotationMatrix, 0, 0, 3, 3, -estimatorDt);
+
+ // Third row. Partial derivative with respect to the orientation state
+ rowStart = offset + errorOrientationIndex;
+ OdometryTools.toSkewSymmetricMatrix(stateVariables.unbiasedGyro, skewMatrix);
+ CommonOps_DDRM.insert(eye3x3, jacobianToPack, rowStart, offset + errorOrientationIndex);
+ MatrixTools.addMatrixBlock(jacobianToPack, rowStart, offset + errorOrientationIndex, skewMatrix, 0, 0, 3, 3, -estimatorDt);
+ if (OdometryKalmanFilter.includeBias)
+ {
+ MatrixTools.setMatrixBlock(jacobianToPack, rowStart, offset + errorGyroBiasIndex, deltaT, 0, 0, 3, 3, -1.0);
+ }
+
+ // Fourth row. Partial derivative with respect to the acceleration bias state
+ CommonOps_DDRM.insert(eye3x3, jacobianToPack, offset + errorAccelBiasIndex, offset + errorAccelBiasIndex);
+ // Fifth row. Partial derivative with respect to the gyro bias state
+ CommonOps_DDRM.insert(eye3x3, jacobianToPack, offset + errorGyroBiasIndex, offset + errorGyroBiasIndex);
+ }
+}
diff --git a/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/SensedVariables.java b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/SensedVariables.java
new file mode 100644
index 000000000000..ce08c1996dae
--- /dev/null
+++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/SensedVariables.java
@@ -0,0 +1,89 @@
+package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF;
+
+import us.ihmc.euclid.referenceFrame.FrameVector3D;
+import us.ihmc.euclid.referenceFrame.ReferenceFrame;
+import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly;
+import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly;
+import us.ihmc.mecano.spatial.Twist;
+import us.ihmc.sensorProcessing.stateEstimation.IMUSensorReadOnly;
+import us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.IMUBiasProvider;
+import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D;
+import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameQuaternion;
+import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D;
+import us.ihmc.yoVariables.registry.YoRegistry;
+import us.ihmc.yoVariables.variable.YoBoolean;
+
+class SensedVariables
+{
+ private static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
+
+ // State providers
+ private final IMUSensorReadOnly baseIMU;
+ private final IMUSensorReadOnly imu;
+ private final IMUBiasProvider imuBiasProvider;
+
+ // State recorders
+ public final YoFrameVector3D gyroMeasurementInWorld;
+ public final YoFrameVector3D gyroMeasurement;
+ public final YoFrameVector3D accelMeasurementInWorld;
+ public final YoFrameVector3D accelMeasurement;
+
+ public final YoFramePoint3D positionMeasurement;
+ public final YoFrameQuaternion orientationMeasurement;
+ public final YoFrameVector3D linearVelocity;
+
+ public final YoBoolean isInContact;
+
+ // Temp variables
+ private final FrameVector3D linearAcceleration = new FrameVector3D();
+ private final FrameVector3D angularVelocity = new FrameVector3D();
+ private final Twist twist = new Twist();
+
+ public SensedVariables(String prefix, IMUSensorReadOnly baseIMU, IMUSensorReadOnly imu, IMUBiasProvider imuBiasProvider, YoRegistry registry)
+ {
+ this.baseIMU = baseIMU;
+ this.imu = imu;
+ this.imuBiasProvider = imuBiasProvider;
+
+ gyroMeasurementInWorld = new YoFrameVector3D(prefix + "GyroMeasurementInWorld", worldFrame, registry);
+ gyroMeasurement = new YoFrameVector3D(prefix + "GyroMeasurement", imu.getMeasurementFrame(), registry);
+ accelMeasurementInWorld = new YoFrameVector3D(prefix + "AccelMeasurementInWorld", worldFrame, registry);
+ accelMeasurement = new YoFrameVector3D(prefix + "AccelMeasurement", imu.getMeasurementFrame(), registry);
+
+ positionMeasurement = new YoFramePoint3D(prefix + "PositionMeasurement", baseIMU.getMeasurementFrame(), registry);
+ orientationMeasurement = new YoFrameQuaternion(prefix + "OrientationMeasurement", baseIMU.getMeasurementFrame(), registry);
+
+ linearVelocity = new YoFrameVector3D(prefix + "LinearVelocity", worldFrame, registry);
+ isInContact = new YoBoolean(prefix + "IsInContact", registry);
+ }
+
+ public void update()
+ {
+ // Update gyro measure
+ FrameVector3DReadOnly gyroBiasInput = imuBiasProvider.getAngularVelocityBiasInIMUFrame(imu);
+ Vector3DReadOnly gyroRawInput = imu.getAngularVelocityMeasurement();
+
+ angularVelocity.setReferenceFrame(imu.getMeasurementFrame());
+ angularVelocity.sub(gyroRawInput, gyroBiasInput);
+
+ gyroMeasurementInWorld.setMatchingFrame(angularVelocity);
+ gyroMeasurement.setMatchingFrame(angularVelocity);
+
+ // Update the accelerometer measure
+ FrameVector3DReadOnly accelBiasInput = imuBiasProvider.getLinearAccelerationBiasInIMUFrame(imu);
+ Vector3DReadOnly accelRawInput = imu.getLinearAccelerationMeasurement();
+
+ linearAcceleration.setReferenceFrame(imu.getMeasurementFrame());
+ linearAcceleration.sub(accelRawInput, accelBiasInput);
+
+ accelMeasurementInWorld.setMatchingFrame(linearAcceleration);
+ accelMeasurement.setMatchingFrame(linearAcceleration);
+
+ positionMeasurement.setFromReferenceFrame(imu.getMeasurementFrame());
+ orientationMeasurement.setFromReferenceFrame(imu.getMeasurementFrame());
+
+ imu.getMeasurementFrame().getTwistRelativeToOther(baseIMU.getMeasurementFrame(), twist);
+ twist.changeFrame(worldFrame);
+ linearVelocity.set(twist.getLinearPart());
+ }
+}
diff --git a/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/StateCorrectionVariables.java b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/StateCorrectionVariables.java
new file mode 100644
index 000000000000..77f95ed757ba
--- /dev/null
+++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/StateCorrectionVariables.java
@@ -0,0 +1,39 @@
+package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF;
+
+import org.ejml.data.DMatrixRMaj;
+import us.ihmc.euclid.referenceFrame.ReferenceFrame;
+import us.ihmc.mecano.frames.MovingReferenceFrame;
+import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D;
+import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D;
+import us.ihmc.yoVariables.registry.YoRegistry;
+
+import static us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF.OdometryIndexHelper.*;
+
+class StateCorrectionVariables
+{
+ private static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
+
+ public final YoFramePoint3D translation;
+ public final YoFrameVector3D linearVelocity;
+ public final YoFrameVector3D orientation;
+ public final YoFrameVector3D accelBias;
+ public final YoFrameVector3D gyroBias;
+
+ public StateCorrectionVariables(String prefix, MovingReferenceFrame sensorFrame, YoRegistry registry)
+ {
+ translation = new YoFramePoint3D(prefix + "TranslationCorrection", worldFrame, registry);
+ linearVelocity = new YoFrameVector3D(prefix + "LinearVelocityCorrection", worldFrame, registry);
+ orientation = new YoFrameVector3D(prefix + "OrientationCorrection", worldFrame, registry);
+ accelBias = new YoFrameVector3D(prefix + "AccelBiasCorrection", sensorFrame, registry);
+ gyroBias = new YoFrameVector3D(prefix + "GyroBiasCorrection", sensorFrame, registry);
+ }
+
+ public void set(int start, DMatrixRMaj state)
+ {
+ translation.set(start + errorTranslationIndex, state);
+ linearVelocity.set(start + errorLinearVelocityIndex, state);
+ orientation.set(start + errorOrientationIndex, state);
+ accelBias.set(start + errorAccelBiasIndex, state);
+ gyroBias.set(start + errorGyroBiasIndex, state);
+ }
+}
diff --git a/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/StateVariables.java b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/StateVariables.java
new file mode 100644
index 000000000000..12e24dc5bd9e
--- /dev/null
+++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/StateVariables.java
@@ -0,0 +1,78 @@
+package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF;
+
+import org.ejml.data.DMatrixRMaj;
+import us.ihmc.euclid.referenceFrame.ReferenceFrame;
+import us.ihmc.mecano.frames.MovingReferenceFrame;
+import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D;
+import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameQuaternion;
+import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D;
+import us.ihmc.yoVariables.registry.YoRegistry;
+
+import static us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF.OdometryIndexHelper.*;
+
+class StateVariables
+{
+ private static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
+
+ public final YoFramePoint3D translation;
+ public final YoFrameVector3D linearVelocity;
+ public final YoFrameQuaternion orientation;
+ public final YoFrameVector3D accelBias;
+ public final YoFrameVector3D gyroBias;
+
+ public final YoFrameVector3D unbiasedAccel;
+ public final YoFrameVector3D unbiasedGyro;
+
+ private final MovingReferenceFrame sensorFrame;
+
+ public StateVariables(String prefix, MovingReferenceFrame sensorFrame, YoRegistry registry)
+ {
+ this.sensorFrame = sensorFrame;
+
+ translation = new YoFramePoint3D(prefix + "Translation", worldFrame, registry);
+ linearVelocity = new YoFrameVector3D(prefix + "LinearVelocity", worldFrame, registry);
+ orientation = new YoFrameQuaternion(prefix + "Orientation", worldFrame, registry);
+ accelBias = new YoFrameVector3D(prefix + "AccelBias", sensorFrame, registry);
+ gyroBias = new YoFrameVector3D(prefix + "GyroBias", sensorFrame, registry);
+ unbiasedAccel = new YoFrameVector3D(prefix + "UnbiasedAccel", sensorFrame, registry);
+ unbiasedGyro = new YoFrameVector3D(prefix + "UnbiasedGyro", sensorFrame, registry);
+ }
+
+ public void initialize()
+ {
+ this.translation.setFromReferenceFrame(sensorFrame);
+ this.orientation.setFromReferenceFrame(sensorFrame);
+ linearVelocity.setMatchingFrame(sensorFrame.getTwistOfFrame().getLinearPart());
+ accelBias.setToZero();
+ gyroBias.setToZero();
+ }
+
+ public void set(int start, DMatrixRMaj state, SensedVariables sensedVariables)
+ {
+ translation.set(start + stateTranslationIndex, state);
+ linearVelocity.set(start + stateLinearVelocityIndex, state);
+ orientation.set(start + stateOrientationIndex, state);
+ accelBias.set(start + stateAccelBiasIndex, state);
+ gyroBias.set(start + stateGyroBiasIndex, state);
+
+ if (OdometryKalmanFilter.includeBias)
+ {
+ unbiasedAccel.sub(sensedVariables.accelMeasurement, accelBias);
+ unbiasedGyro.sub(sensedVariables.gyroMeasurement, gyroBias);
+ }
+ else
+ {
+ unbiasedAccel.set(sensedVariables.accelMeasurement);
+ unbiasedGyro.set(sensedVariables.gyroMeasurement);
+ }
+ }
+
+ public void get(int start, DMatrixRMaj stateToPack)
+ {
+ translation.get(start + stateTranslationIndex, stateToPack);
+ linearVelocity.get(start + stateLinearVelocityIndex, stateToPack);
+ orientation.get(start + stateOrientationIndex, stateToPack);
+ accelBias.get(start + stateAccelBiasIndex, stateToPack);
+ gyroBias.get(start + stateGyroBiasIndex, stateToPack);
+ }
+}
diff --git a/ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/MeasurementModelTest.java b/ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/MeasurementModelTest.java
new file mode 100644
index 000000000000..49bf96e8b6fc
--- /dev/null
+++ b/ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/MeasurementModelTest.java
@@ -0,0 +1,435 @@
+package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF;
+
+import org.ejml.EjmlUnitTests;
+import org.ejml.data.DMatrixRMaj;
+import org.ejml.dense.row.CommonOps_DDRM;
+import org.junit.jupiter.api.Test;
+import us.ihmc.euclid.geometry.Pose3D;
+import us.ihmc.euclid.referenceFrame.FrameVector3D;
+import us.ihmc.euclid.referenceFrame.ReferenceFrame;
+import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly;
+import us.ihmc.euclid.tools.EuclidCoreRandomTools;
+import us.ihmc.euclid.transform.RigidBodyTransform;
+import us.ihmc.euclid.tuple3D.Vector3D;
+import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly;
+import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly;
+import us.ihmc.matrixlib.MatrixTools;
+import us.ihmc.mecano.frames.MovingReferenceFrame;
+import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
+import us.ihmc.mecano.spatial.Twist;
+import us.ihmc.sensorProcessing.stateEstimation.IMUSensorReadOnly;
+import us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.IMUBiasProvider;
+import us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF.ProcessModelTest.TestBiasProvider;
+import us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF.ProcessModelTest.TestIMU;
+import us.ihmc.yoVariables.registry.YoRegistry;
+
+import java.util.Random;
+
+import static org.junit.jupiter.api.Assertions.*;
+
+public class MeasurementModelTest
+{
+ private static final double gravityZ = 9.81;
+ private static final FrameVector3D gravity = new FrameVector3D(ReferenceFrame.getWorldFrame(), 0.0, 0.0, gravityZ);
+ private static final double estimatorDt = 0.01;
+
+ @Test
+ public void testUpdate()
+ {
+ int iters = 100;
+ Random random = new Random(1738L);
+
+ YoRegistry test = new YoRegistry(getClass().getSimpleName());
+ ProcessModelTest.TestIMU baseIMU = new ProcessModelTest.TestIMU("base");
+ ProcessModelTest.TestIMU footIMU = new ProcessModelTest.TestIMU("foot");
+
+ StateVariables baseState = new StateVariables("base", baseIMU.getMeasurementFrame(), test);
+ StateVariables footState = new StateVariables("foot", footIMU.getMeasurementFrame(), test);
+ SensedVariables baseSensing = new SensedVariables("baseSensed", baseIMU, baseIMU, new TestBiasProvider(), test);
+ SensedVariables footSensing = new SensedVariables("footSensed", baseIMU, footIMU, new TestBiasProvider(), test);
+ MeasurementVariables footMeasure = new MeasurementVariables("footMeasure", test);
+
+ MeasurementModel measurementModel = new MeasurementModel("foot0", baseState, footState, footSensing, footMeasure, gravity, () -> Double.MAX_VALUE, test);
+ DMatrixRMaj jacobian = new DMatrixRMaj(OdometryIndexHelper.errorSizePerLink, OdometryIndexHelper.errorSizePerLink * 2);
+
+ for (int iter = 0; iter < iters; iter++)
+ {
+ jacobian.zero();
+
+ OdomTestTools.setRandomSensed(random, footSensing);
+ footIMU.linearAcceleration.set(footSensing.accelMeasurement);
+ footIMU.angularVelocity.set(footSensing.gyroMeasurement);
+ footIMU.pose.set(EuclidCoreRandomTools.nextPoint3D(random, 10.0), EuclidCoreRandomTools.nextOrientation3D(random));
+ footIMU.linearVelocity.set(EuclidCoreRandomTools.nextVector3D(random, 10.0));
+ footIMU.sensorFrame.update();
+
+ OdomTestTools.setRandomSensed(random, baseSensing);
+ baseIMU.linearAcceleration.set(baseSensing.accelMeasurement);
+ baseIMU.angularVelocity.set(baseSensing.gyroMeasurement);
+ baseIMU.pose.set(EuclidCoreRandomTools.nextPoint3D(random, 10.0), EuclidCoreRandomTools.nextOrientation3D(random));
+ baseIMU.linearVelocity.set(EuclidCoreRandomTools.nextVector3D(random, 10.0));
+ baseIMU.sensorFrame.update();
+
+ OdomTestTools.setRandomState(random, baseSensing, baseState);
+ OdomTestTools.setRandomState(random, footSensing, footState);
+
+ measurementModel.update(new DMatrixRMaj(3, 3));
+
+ DMatrixRMaj baseTranslation = new DMatrixRMaj(3, 1);
+ DMatrixRMaj footTranslation = new DMatrixRMaj(3, 1);
+ DMatrixRMaj footRelativePosition = new DMatrixRMaj(3, 1);
+ DMatrixRMaj baseLinearVelocity = new DMatrixRMaj(3, 1);
+ DMatrixRMaj footLinearVelocity = new DMatrixRMaj(3, 1);
+ DMatrixRMaj gravityVector = new DMatrixRMaj(3, 1);
+ DMatrixRMaj baseRotationMatrix = new DMatrixRMaj(3, 3);
+ DMatrixRMaj footRotationMatrix = new DMatrixRMaj(3, 3);
+ DMatrixRMaj baseAcceleration = new DMatrixRMaj(3, 1);
+ DMatrixRMaj baseAccelerationBias = new DMatrixRMaj(3, 1);
+ DMatrixRMaj footAccelerationBias = new DMatrixRMaj(3, 1);
+ DMatrixRMaj baseGyro = new DMatrixRMaj(3, 1);
+ DMatrixRMaj baseGyroBias = new DMatrixRMaj(3, 1);
+ DMatrixRMaj baseGyroUnbiased = new DMatrixRMaj(3, 1);
+ DMatrixRMaj footVelocity = new DMatrixRMaj(3, 1);
+
+ DMatrixRMaj footOffset = new DMatrixRMaj(3, 1);
+ DMatrixRMaj skewMatrix = new DMatrixRMaj(3, 3);
+
+ baseState.translation.get(baseTranslation);
+ footState.translation.get(footTranslation);
+ footSensing.positionMeasurement.get(footRelativePosition);
+ baseState.linearVelocity.get(baseLinearVelocity);
+ footState.linearVelocity.get(footLinearVelocity);
+ OdometryTools.toRotationMatrix(baseState.orientation, baseRotationMatrix);
+ OdometryTools.toRotationMatrix(footState.orientation, footRotationMatrix);
+ gravity.get(gravityVector);
+ baseSensing.accelMeasurement.get(baseAcceleration);
+ baseState.accelBias.get(baseAccelerationBias);
+ footState.accelBias.get(footAccelerationBias);
+ baseSensing.gyroMeasurement.get(baseGyro);
+ baseState.gyroBias.get(baseGyroBias);
+
+ DMatrixRMaj expectedRelativeError = new DMatrixRMaj(3, 1);
+ DMatrixRMaj expectedPredictedLinearVelocityError = new DMatrixRMaj(3, 1);
+ DMatrixRMaj expectedPredictedRotationError = new DMatrixRMaj(4, 1);
+ DMatrixRMaj expectedPredictedContactVelocity = new DMatrixRMaj(3, 1);
+ DMatrixRMaj expectedPredictedContactAccel = new DMatrixRMaj(3, 1);
+
+ // First term
+ CommonOps_DDRM.subtract(footTranslation, baseTranslation, footOffset);
+ CommonOps_DDRM.multTransA(baseRotationMatrix, footOffset, expectedRelativeError);
+
+ // Second term
+ DMatrixRMaj rotation = multiplyLeftInverse(footSensing.orientationMeasurement, OdomTestTools.fromVector(multiplyLeftInverse(baseState.orientation, footState.orientation)));
+ expectedPredictedRotationError.set(OdometryTools.logMap(rotation));
+
+ // Third term
+ CommonOps_DDRM.subtract(baseLinearVelocity, footLinearVelocity, footVelocity);
+ if (OdometryKalmanFilter.includeBias)
+ CommonOps_DDRM.subtract(baseGyro, baseGyroBias, baseGyroUnbiased);
+ else
+ baseGyroUnbiased.set(baseGyro);
+ OdometryTools.toSkewSymmetricMatrix(baseGyroUnbiased.get(0), baseGyroUnbiased.get(1), baseGyroUnbiased.get(2), skewMatrix);
+ footSensing.linearVelocity.get(expectedPredictedLinearVelocityError);
+ CommonOps_DDRM.multAdd(-1.0, skewMatrix, footRelativePosition, expectedPredictedLinearVelocityError);
+ CommonOps_DDRM.multAddTransA(-1.0, baseRotationMatrix, footVelocity, expectedPredictedLinearVelocityError);
+
+ // Fourth term
+ expectedPredictedContactVelocity.set(footLinearVelocity);
+
+
+ // Fifth term
+ CommonOps_DDRM.multTransA(footRotationMatrix, gravityVector, expectedPredictedContactAccel);
+ if (OdometryKalmanFilter.includeBias)
+ CommonOps_DDRM.addEquals(expectedPredictedContactAccel, footAccelerationBias);
+
+ DMatrixRMaj predictedRelativeError = new DMatrixRMaj(3, 1);
+ DMatrixRMaj predictedRotationError = new DMatrixRMaj(3, 1);
+ DMatrixRMaj predictedLinearVelocityError = new DMatrixRMaj(3, 1);
+ DMatrixRMaj predictedContactVelocity = new DMatrixRMaj(3, 1);
+ DMatrixRMaj predictedContactAccel = new DMatrixRMaj(3, 1);
+
+ footMeasure.relativePosition.get(predictedRelativeError);
+ footMeasure.relativeOrientationError.get(predictedRotationError);
+ footMeasure.relativeLinearVelocityError.get(predictedLinearVelocityError);
+ footMeasure.contactVelocity.get(predictedContactVelocity);
+ footMeasure.accelMeasure.get(predictedContactAccel);
+
+ EjmlUnitTests.assertEquals(expectedRelativeError, predictedRelativeError, 1e-6);
+ EjmlUnitTests.assertEquals(expectedPredictedRotationError, predictedRotationError, 1e-6);
+ EjmlUnitTests.assertEquals(expectedPredictedLinearVelocityError, predictedLinearVelocityError, 1e-6);
+ EjmlUnitTests.assertEquals(expectedPredictedContactVelocity, predictedContactVelocity, 1e-6);
+ EjmlUnitTests.assertEquals(expectedPredictedContactAccel, predictedContactAccel, 1e-6);
+ }
+ }
+
+
+ @Test
+ public void testComputeMeasurementJacobian()
+ {
+ int iters = 100;
+ Random random = new Random(1738L);
+
+ YoRegistry test = new YoRegistry(getClass().getSimpleName());
+ TestIMU baseIMU = new TestIMU("base");
+ TestIMU footIMU = new TestIMU("foot");
+
+ StateVariables baseState = new StateVariables("base", baseIMU.getMeasurementFrame(), test);
+ StateVariables footState = new StateVariables("foot", footIMU.getMeasurementFrame(), test);
+ SensedVariables baseSensing = new SensedVariables("baseSensed", baseIMU, baseIMU, new TestBiasProvider(), test);
+ SensedVariables footSensing = new SensedVariables("footSensed", baseIMU, footIMU, new TestBiasProvider(), test);
+ MeasurementVariables measurement = new MeasurementVariables("footMeasure", test);
+
+ MeasurementModel measurementModel = new MeasurementModel("foot0", baseState, footState, footSensing, measurement, gravity, () -> Double.MAX_VALUE, test);
+ DMatrixRMaj jacobian = new DMatrixRMaj(OdometryIndexHelper.errorSizePerLink, OdometryIndexHelper.errorSizePerLink * 2);
+
+ for (int iter = 0; iter < iters; iter++)
+ {
+ jacobian.zero();
+
+ measurementModel.isInContact.set(true);
+
+ OdomTestTools.setRandomSensed(random, footSensing);
+ footIMU.linearAcceleration.set(footSensing.accelMeasurement);
+ footIMU.angularVelocity.set(footSensing.gyroMeasurement);
+ footIMU.pose.set(EuclidCoreRandomTools.nextPoint3D(random, 10.0), EuclidCoreRandomTools.nextOrientation3D(random));
+ footIMU.linearVelocity.set(EuclidCoreRandomTools.nextVector3D(random, 10.0));
+ footIMU.sensorFrame.update();
+
+ OdomTestTools.setRandomSensed(random, baseSensing);
+ baseIMU.linearAcceleration.set(baseSensing.accelMeasurement);
+ baseIMU.angularVelocity.set(baseSensing.gyroMeasurement);
+ baseIMU.pose.set(EuclidCoreRandomTools.nextPoint3D(random, 10.0), EuclidCoreRandomTools.nextOrientation3D(random));
+ baseIMU.linearVelocity.set(EuclidCoreRandomTools.nextVector3D(random, 10.0));
+ baseIMU.sensorFrame.update();
+
+ OdomTestTools.setRandomState(random, baseSensing, baseState);
+ OdomTestTools.setRandomState(random, footSensing, footState);
+
+ measurementModel.computeBaseMeasurementJacobian( 0, jacobian);
+ measurementModel.computeFootMeasurementJacobian( 0, OdometryIndexHelper.errorSizePerLink, jacobian);
+
+ DMatrixRMaj jacobianExpected = new DMatrixRMaj(OdometryIndexHelper.errorSizePerLink, 2 * OdometryIndexHelper.errorSizePerLink);
+ DMatrixRMaj skewMatrix = new DMatrixRMaj(3, 3);
+ DMatrixRMaj baseRotationMatrix = new DMatrixRMaj(3, 3);
+ DMatrixRMaj baseRotationMatrixTranspose = new DMatrixRMaj(3, 3);
+ DMatrixRMaj footRotationMatrix = new DMatrixRMaj(3, 3);
+ DMatrixRMaj footRotationMatrixTranspose = new DMatrixRMaj(3, 3);
+ OdometryTools.toRotationMatrix(baseState.orientation, baseRotationMatrix);
+ OdometryTools.toRotationMatrix(footState.orientation, footRotationMatrix);
+ CommonOps_DDRM.transpose(baseRotationMatrix, baseRotationMatrixTranspose);
+ CommonOps_DDRM.transpose(footRotationMatrix, footRotationMatrixTranspose);
+
+ // Row 1, base
+ MatrixTools.setMatrixBlock(jacobianExpected, 0, 0, baseRotationMatrixTranspose, 0, 0, 3, 3, -1.0);
+ DMatrixRMaj footPosition = new DMatrixRMaj(3, 1);
+ DMatrixRMaj basePosition = new DMatrixRMaj(3, 1);
+ DMatrixRMaj footOffset = new DMatrixRMaj(3, 1);
+ DMatrixRMaj footOffsetInBaseFrame = new DMatrixRMaj(3, 1);
+ footState.translation.get(footPosition);
+ baseState.translation.get(basePosition);
+ CommonOps_DDRM.subtract(footPosition, basePosition, footOffset);
+ CommonOps_DDRM.multTransA(baseRotationMatrix, footOffset, footOffsetInBaseFrame);
+ OdometryTools.toSkewSymmetricMatrix(footOffsetInBaseFrame.get(0), footOffsetInBaseFrame.get(1), footOffsetInBaseFrame.get(2), skewMatrix);
+ CommonOps_DDRM.insert(skewMatrix, jacobianExpected, 0, 6);
+
+ // Row 1, foot
+ CommonOps_DDRM.insert(baseRotationMatrixTranspose, jacobianExpected, 0, 15);
+
+ // Row 2, base
+ DMatrixRMaj left = OdometryTools.lOperator(multiplyLeftInverse(footState.orientation, baseState.orientation));
+ DMatrixRMaj right = OdometryTools.rOperator(footSensing.orientationMeasurement);
+ DMatrixRMaj block = new DMatrixRMaj(4, 4);
+ CommonOps_DDRM.mult(left, right, block);
+ MatrixTools.setMatrixBlock(jacobianExpected, 3, 6, block, 1, 1, 3, 3, -1.0);
+
+ // Row 2, foot
+ block = OdometryTools.lOperator(multiply(footSensing.orientationMeasurement, OdomTestTools.fromVector(multiplyLeftInverse(baseState.orientation, footState.orientation))));
+ MatrixTools.setMatrixBlock(jacobianExpected, 3, 21, block, 1, 1, 3, 3, 1.0);
+
+ // Row 3, base
+ MatrixTools.setMatrixBlock(jacobianExpected, 6, 3, baseRotationMatrixTranspose, 0, 0, 3, 3, -1.0);
+ DMatrixRMaj footVelocity = new DMatrixRMaj(3, 1);
+ DMatrixRMaj baseVelocity = new DMatrixRMaj(3, 1);
+ DMatrixRMaj velocityError = new DMatrixRMaj(3, 1);
+ DMatrixRMaj velocityErrorInBaseFrame = new DMatrixRMaj(3, 1);
+ footState.linearVelocity.get(footVelocity);
+ baseState.linearVelocity.get(baseVelocity);
+ CommonOps_DDRM.subtract(footVelocity, baseVelocity, velocityError);
+ CommonOps_DDRM.multTransA(baseRotationMatrix, velocityError, velocityErrorInBaseFrame);
+ OdometryTools.toSkewSymmetricMatrix(velocityErrorInBaseFrame.get(0), velocityErrorInBaseFrame.get(1), velocityErrorInBaseFrame.get(2), skewMatrix);
+ CommonOps_DDRM.insert(skewMatrix, jacobianExpected, 6, 6);
+ if (OdometryKalmanFilter.includeBias)
+ {
+ OdometryTools.toSkewSymmetricMatrix(footSensing.positionMeasurement, skewMatrix);
+ MatrixTools.setMatrixBlock(jacobianExpected, 6, 12, skewMatrix, 0, 0, 3, 3, -1.0);
+ }
+
+ // Row 3, foot
+ CommonOps_DDRM.insert(baseRotationMatrixTranspose, jacobianExpected, 6, 18);
+
+ // Row 4, foot
+ if (OdometryKalmanFilter.includeBias)
+ {
+ for (int i = 0; i < 3; i++)
+ jacobianExpected.set(9 + i, 18 + i, 1.0);
+ }
+
+ // Row 5, foot
+ if (OdometryKalmanFilter.includeBias)
+ {
+ DMatrixRMaj gravityVector = new DMatrixRMaj(3, 1);
+ DMatrixRMaj rotatedGravityVector = new DMatrixRMaj(3, 1);
+ gravity.get(gravityVector);
+ CommonOps_DDRM.multTransA(footRotationMatrix, gravityVector, rotatedGravityVector);
+ OdometryTools.toSkewSymmetricMatrix(rotatedGravityVector.get(0), rotatedGravityVector.get(1), rotatedGravityVector.get(2), skewMatrix);
+ CommonOps_DDRM.insert(skewMatrix, jacobianExpected, 12, 21);
+
+ for (int i = 0; i < 3; i++)
+ jacobianExpected.set(12 + i, 24 + i, 1.0);
+ }
+
+ EjmlUnitTests.assertEquals(jacobianExpected, jacobian, 1e-5);
+ }
+ }
+
+ static DMatrixRMaj multiply(QuaternionReadOnly a, QuaternionReadOnly b)
+ {
+ DMatrixRMaj vector = new DMatrixRMaj(4, 1);
+ CommonOps_DDRM.mult(OdometryTools.lOperator(a), OdomTestTools.toVector(b), vector);
+
+ return vector;
+ }
+
+ static DMatrixRMaj multiplyLeftInverse(QuaternionReadOnly a, QuaternionReadOnly b)
+ {
+ DMatrixRMaj vector = new DMatrixRMaj(4, 1);
+ CommonOps_DDRM.mult(OdometryTools.lOperator(a.getS(), -a.getX(), -a.getY(), -a.getZ()), OdomTestTools.toVector(b), vector);
+
+ return vector;
+ }
+
+ static class TestBiasProvider implements IMUBiasProvider
+ {
+
+ @Override
+ public FrameVector3DReadOnly getAngularVelocityBiasInIMUFrame(IMUSensorReadOnly imu)
+ {
+ return new FrameVector3D(imu.getMeasurementFrame());
+ }
+
+ @Override
+ public FrameVector3DReadOnly getAngularVelocityBiasInWorldFrame(IMUSensorReadOnly imu)
+ {
+ return new FrameVector3D();
+ }
+
+ @Override
+ public FrameVector3DReadOnly getLinearAccelerationBiasInIMUFrame(IMUSensorReadOnly imu)
+ {
+ return new FrameVector3D(imu.getMeasurementFrame());
+ }
+
+ @Override
+ public FrameVector3DReadOnly getLinearAccelerationBiasInWorldFrame(IMUSensorReadOnly imu)
+ {
+ return new FrameVector3D();
+ }
+ }
+
+ static class TestIMU implements IMUSensorReadOnly
+ {
+ private String name;
+ public final MovingReferenceFrame sensorFrame;
+ public final Pose3D pose = new Pose3D();
+ public final Vector3D angularVelocity = new Vector3D();
+ public final Vector3D linearVelocity = new Vector3D();
+ public final Vector3D linearAcceleration = new Vector3D();
+
+ public TestIMU(String name)
+ {
+ this.name = name;
+
+ sensorFrame = new MovingReferenceFrame(name, ReferenceFrame.getWorldFrame())
+ {
+ @Override
+ protected void updateTwistRelativeToParent(Twist twist)
+ {
+ twist.setToZero();
+ pose.transform(linearVelocity, twist.getLinearPart());
+ }
+
+ @Override
+ protected void updateTransformToParent(RigidBodyTransform rigidBodyTransform)
+ {
+ rigidBodyTransform.set(pose);
+ }
+ };
+ }
+
+ @Override
+ public String getSensorName()
+ {
+ return name;
+ }
+
+ @Override
+ public MovingReferenceFrame getMeasurementFrame()
+ {
+ return sensorFrame;
+ }
+
+ @Override
+ public RigidBodyBasics getMeasurementLink()
+ {
+ return null;
+ }
+
+ @Override
+ public QuaternionReadOnly getOrientationMeasurement()
+ {
+ return pose.getOrientation();
+ }
+
+ @Override
+ public Vector3DReadOnly getAngularVelocityMeasurement()
+ {
+ return angularVelocity;
+ }
+
+ @Override
+ public Vector3DReadOnly getLinearAccelerationMeasurement()
+ {
+ return linearAcceleration;
+ }
+
+ @Override
+ public void getOrientationNoiseCovariance(DMatrixRMaj noiseCovarianceToPack)
+ {
+
+ }
+
+ @Override
+ public void getAngularVelocityNoiseCovariance(DMatrixRMaj noiseCovarianceToPack)
+ {
+
+ }
+
+ @Override
+ public void getAngularVelocityBiasProcessNoiseCovariance(DMatrixRMaj biasProcessNoiseCovarianceToPack)
+ {
+
+ }
+
+ @Override
+ public void getLinearAccelerationNoiseCovariance(DMatrixRMaj noiseCovarianceToPack)
+ {
+
+ }
+
+ @Override
+ public void getLinearAccelerationBiasProcessNoiseCovariance(DMatrixRMaj biasProcessNoiseCovarianceToPack)
+ {
+
+ }
+ }
+}
diff --git a/ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdomTestTools.java b/ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdomTestTools.java
new file mode 100644
index 000000000000..41e7dadfa740
--- /dev/null
+++ b/ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdomTestTools.java
@@ -0,0 +1,67 @@
+package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF;
+
+import org.ejml.data.DMatrixRMaj;
+import us.ihmc.euclid.tools.EuclidCoreRandomTools;
+import us.ihmc.euclid.tools.EuclidCoreTools;
+import us.ihmc.euclid.tuple4D.Quaternion;
+import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly;
+
+import java.util.Random;
+
+public class OdomTestTools
+{
+ public static void setRandomSensed(Random random, SensedVariables sensedVariablesToPack)
+ {
+ sensedVariablesToPack.gyroMeasurement.set(EuclidCoreRandomTools.nextVector3D(random, 5.0));
+ sensedVariablesToPack.accelMeasurement.set(EuclidCoreRandomTools.nextVector3D(random, 5.0));
+ sensedVariablesToPack.positionMeasurement.set(EuclidCoreRandomTools.nextVector3D(random, 5.0));
+ sensedVariablesToPack.orientationMeasurement.set(EuclidCoreRandomTools.nextQuaternion(random, 5.0));
+ sensedVariablesToPack.linearVelocity.set(EuclidCoreRandomTools.nextVector3D(random, 5.0));
+ }
+
+ public static DMatrixRMaj setRandomState(Random random, SensedVariables sensedVariables, StateVariables stateVariablesToPack)
+ {
+ stateVariablesToPack.translation.set(EuclidCoreRandomTools.nextPoint3D(random, 10.0));
+ stateVariablesToPack.linearVelocity.set(EuclidCoreRandomTools.nextVector3D(random, 10.0));
+ stateVariablesToPack.orientation.set(EuclidCoreRandomTools.nextQuaternion(random));
+ stateVariablesToPack.accelBias.set(EuclidCoreRandomTools.nextVector3D(random, 5.0));
+ stateVariablesToPack.gyroBias.set(EuclidCoreRandomTools.nextVector3D(random, 5.0));
+
+ if (OdometryKalmanFilter.includeBias)
+ {
+ stateVariablesToPack.unbiasedAccel.sub(sensedVariables.accelMeasurement, stateVariablesToPack.accelBias);
+ stateVariablesToPack.unbiasedGyro.sub(sensedVariables.gyroMeasurement, stateVariablesToPack.gyroBias);
+ }
+ else
+ {
+ stateVariablesToPack.unbiasedAccel.set(sensedVariables.accelMeasurement);
+ stateVariablesToPack.unbiasedGyro.set(sensedVariables.gyroMeasurement);
+ }
+
+ DMatrixRMaj stateVector = new DMatrixRMaj(OdometryIndexHelper.stateSizePerLink, 1);
+ stateVariablesToPack.translation.get(stateVector);
+ stateVariablesToPack.linearVelocity.get(3, stateVector);
+ stateVariablesToPack.orientation.get(6, stateVector);
+ stateVariablesToPack.accelBias.get(10, stateVector);
+ stateVariablesToPack.gyroBias.get(13, stateVector);
+
+ return stateVector;
+ }
+
+
+ static Quaternion fromVector(DMatrixRMaj quaternion)
+ {
+ return new Quaternion(quaternion.get(1), quaternion.get(2), quaternion.get(3), quaternion.get(0));
+ }
+
+ static DMatrixRMaj toVector(QuaternionReadOnly quaternionReadOnly)
+ {
+ DMatrixRMaj vector = new DMatrixRMaj(4, 1);
+ vector.set(0, quaternionReadOnly.getS());
+ vector.set(1, quaternionReadOnly.getX());
+ vector.set(2, quaternionReadOnly.getY());
+ vector.set(3, quaternionReadOnly.getZ());
+
+ return vector;
+ }
+}
diff --git a/ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryToolsTest.java b/ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryToolsTest.java
new file mode 100644
index 000000000000..ae6d107be4ee
--- /dev/null
+++ b/ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryToolsTest.java
@@ -0,0 +1,151 @@
+package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF;
+
+
+import org.ejml.EjmlUnitTests;
+import org.ejml.data.DMatrixRMaj;
+import org.ejml.dense.row.CommonOps_DDRM;
+import org.junit.jupiter.api.Test;
+import us.ihmc.euclid.matrix.RotationMatrix;
+import us.ihmc.euclid.tools.EuclidCoreRandomTools;
+import us.ihmc.euclid.tools.EuclidCoreTestTools;
+import us.ihmc.euclid.tuple3D.Vector3D;
+import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly;
+import us.ihmc.euclid.tuple4D.Quaternion;
+import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly;
+import us.ihmc.matrixlib.MatrixTools;
+
+import java.util.Random;
+
+public class OdometryToolsTest
+{
+ @Test
+ public void testToSkewSymmetric()
+ {
+ Random random = new Random(1738L);
+
+ for (int i = 0; i < 1000; i++)
+ {
+ Vector3D a = EuclidCoreRandomTools.nextVector3D(random);
+ Vector3D b = EuclidCoreRandomTools.nextVector3D(random);
+ Vector3D cExpected = EuclidCoreRandomTools.nextVector3D(random);
+ Vector3D c = EuclidCoreRandomTools.nextVector3D(random);
+
+ cExpected.cross(a, b);
+
+ DMatrixRMaj skewMatrix = new DMatrixRMaj(3, 3);
+
+ DMatrixRMaj bVector = new DMatrixRMaj(3, 1);
+ b.get(bVector);
+ DMatrixRMaj cVector = new DMatrixRMaj(3, 1);
+ OdometryTools.toSkewSymmetricMatrix(a, skewMatrix);
+
+ CommonOps_DDRM.mult(skewMatrix, bVector, cVector);
+ c.set(cVector);
+
+ EuclidCoreTestTools.assertEquals(cExpected, c, 1e-8);
+ }
+ }
+
+ @Test
+ public void testTransform()
+ {
+ Random random = new Random(1738L);
+
+ for (int i = 0; i < 1000; i++)
+ {
+ Quaternion a = EuclidCoreRandomTools.nextQuaternion(random);
+ Vector3D b = EuclidCoreRandomTools.nextVector3D(random);
+ Vector3D cExpected = EuclidCoreRandomTools.nextVector3D(random);
+ Vector3D c = EuclidCoreRandomTools.nextVector3D(random);
+ Vector3D cInverseExpected = EuclidCoreRandomTools.nextVector3D(random);
+ Vector3D cInverse = EuclidCoreRandomTools.nextVector3D(random);
+
+ DMatrixRMaj rotationMatrix = new DMatrixRMaj(3, 3);
+ DMatrixRMaj rotationMatrixTranspose = new DMatrixRMaj(3, 3);
+ DMatrixRMaj bVector = new DMatrixRMaj(3, 1);
+ DMatrixRMaj cVector = new DMatrixRMaj(3, 1);
+ DMatrixRMaj cInverseVector = new DMatrixRMaj(3, 1);
+ OdometryTools.toRotationMatrix(a, rotationMatrix);
+ CommonOps_DDRM.transpose(rotationMatrix, rotationMatrixTranspose);
+ OdometryTools.toRotationMatrix(a, rotationMatrix);
+ b.get(bVector);
+
+ CommonOps_DDRM.mult(rotationMatrix, bVector, cVector);
+ CommonOps_DDRM.mult(rotationMatrixTranspose, bVector, cInverseVector);
+ c.set(cVector);
+ cInverse.set(cInverseVector);
+
+ a.transform(b, cExpected);
+ a.inverseTransform(b, cInverseExpected);
+
+ EuclidCoreTestTools.assertEquals(cExpected, c, 1e-5);
+ EuclidCoreTestTools.assertEquals(cInverseExpected, cInverse, 1e-5);
+ }
+ }
+
+ @Test
+ public void testToRotationMatrix()
+ {
+ Random random = new Random(1738L);
+
+ for (int i = 0; i < 1000; i++)
+ {
+ Quaternion quaternion = EuclidCoreRandomTools.nextQuaternion(random);
+ DMatrixRMaj rotationMatrix = new DMatrixRMaj(3, 3);
+
+ OdometryTools.toRotationMatrix(quaternion, rotationMatrix);
+
+ RotationMatrix mat = new RotationMatrix(quaternion);
+ DMatrixRMaj expected = new DMatrixRMaj(3, 3);
+ mat.get(expected);
+
+ EjmlUnitTests.assertEquals(expected, rotationMatrix, 1e-8);
+ }
+ }
+
+ @Test
+ public void testToRotationMatrixInverse()
+ {
+ Random random = new Random(1738L);
+
+ for (int i = 0; i < 1000; i++)
+ {
+ Quaternion quaternion = EuclidCoreRandomTools.nextQuaternion(random);
+ DMatrixRMaj rotationMatrixInverse = new DMatrixRMaj(3, 3);
+
+ OdometryTools.toRotationMatrixInverse(quaternion, rotationMatrixInverse);
+
+ RotationMatrix mat = new RotationMatrix(quaternion);
+ DMatrixRMaj rotationExpected = new DMatrixRMaj(3, 3);
+ mat.get(rotationExpected);
+
+ CommonOps_DDRM.invert(rotationExpected);
+
+ EjmlUnitTests.assertEquals(rotationExpected, rotationMatrixInverse, 1e-8);
+ }
+ }
+
+ @Test
+ public void testExponentialMap()
+ {
+ Random random = new Random(1738L);
+
+ for (int i = 0; i < 1000; i++)
+ {
+ Quaternion a = EuclidCoreRandomTools.nextQuaternion(random);
+ Vector3DReadOnly rotationVector = EuclidCoreRandomTools.nextVector3D(random, 0.02);
+ DMatrixRMaj rotatedAVector = new DMatrixRMaj(4, 1);
+ DMatrixRMaj rotationQuat = OdometryTools.exponentialMap(rotationVector);
+ CommonOps_DDRM.mult(OdometryTools.lOperator(a), OdometryTools.exponentialMap(rotationVector), rotatedAVector);
+ Quaternion rotatedA = OdomTestTools.fromVector(rotatedAVector);
+
+ Quaternion rotationQuaternion = new Quaternion(rotationVector);
+
+ EuclidCoreTestTools.assertEquals(rotationQuaternion, OdomTestTools.fromVector(rotationQuat), 1e-5);
+ Quaternion rotatedAExpected = new Quaternion();
+ rotatedAExpected.multiply(a,rotationQuaternion);
+
+ EuclidCoreTestTools.assertEquals(rotatedAExpected, rotatedA, 1e-6);
+ }
+ }
+}
diff --git a/ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/ProcessModelTest.java b/ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/ProcessModelTest.java
new file mode 100644
index 000000000000..2e38e7920f91
--- /dev/null
+++ b/ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/ProcessModelTest.java
@@ -0,0 +1,348 @@
+package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF;
+
+import org.ejml.EjmlUnitTests;
+import org.ejml.data.DMatrixRMaj;
+import org.ejml.dense.row.CommonOps_DDRM;
+import org.junit.jupiter.api.Test;
+import us.ihmc.euclid.geometry.Pose3D;
+import us.ihmc.euclid.referenceFrame.FrameVector3D;
+import us.ihmc.euclid.referenceFrame.ReferenceFrame;
+import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly;
+import us.ihmc.euclid.tools.EuclidCoreRandomTools;
+import us.ihmc.euclid.transform.RigidBodyTransform;
+import us.ihmc.euclid.tuple3D.Vector3D;
+import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly;
+import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly;
+import us.ihmc.log.LogTools;
+import us.ihmc.matrixlib.MatrixTools;
+import us.ihmc.mecano.frames.MovingReferenceFrame;
+import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
+import us.ihmc.mecano.spatial.Twist;
+import us.ihmc.sensorProcessing.stateEstimation.IMUSensorReadOnly;
+import us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.IMUBiasProvider;
+import us.ihmc.yoVariables.registry.YoRegistry;
+
+import java.util.Random;
+
+public class ProcessModelTest
+{
+ private static final double gravityZ = 9.81;
+ private static final FrameVector3D gravity = new FrameVector3D(ReferenceFrame.getWorldFrame(), 0.0, 0.0, gravityZ);
+ private static final double estimatorDt = 0.01;
+
+ @Test
+ public void testUpdate()
+ {
+ int iters = 100;
+ Random random = new Random(1738L);
+
+ YoRegistry test = new YoRegistry(getClass().getSimpleName());
+ TestIMU baseIMU = new TestIMU("base");
+ TestIMU footIMU = new TestIMU("foot");
+
+ StateVariables processState = new StateVariables("process", footIMU.getMeasurementFrame(), test);
+ StateVariables predictedState = new StateVariables("predicted", footIMU.getMeasurementFrame(), test);
+ SensedVariables sensedVariables = new SensedVariables("foot", baseIMU, footIMU, new TestBiasProvider(), test);
+
+ ProcessModel processModel = new ProcessModel(processState, predictedState, () -> true, gravity, estimatorDt);
+ DMatrixRMaj jacobian = new DMatrixRMaj(OdometryIndexHelper.errorSizePerLink, OdometryIndexHelper.errorSizePerLink);
+
+ for (int iter = 0; iter < iters; iter++)
+ {
+ OdomTestTools.setRandomSensed(random, sensedVariables);
+ footIMU.linearAcceleration.set(sensedVariables.accelMeasurement);
+ footIMU.angularVelocity.set(sensedVariables.gyroMeasurement);
+ footIMU.pose.set(EuclidCoreRandomTools.nextPoint3D(random, 10.0), EuclidCoreRandomTools.nextOrientation3D(random));
+ footIMU.linearVelocity.set(EuclidCoreRandomTools.nextVector3D(random, 10.0));
+ footIMU.sensorFrame.update();
+ baseIMU.pose.set(EuclidCoreRandomTools.nextPoint3D(random, 10.0), EuclidCoreRandomTools.nextOrientation3D(random));
+ baseIMU.linearVelocity.set(EuclidCoreRandomTools.nextVector3D(random, 10.0));
+ baseIMU.sensorFrame.update();
+
+ OdomTestTools.setRandomState(random, sensedVariables, processState);
+ OdomTestTools.setRandomState(random, sensedVariables, predictedState);
+
+ processModel.update();
+
+ DMatrixRMaj translation = new DMatrixRMaj(3, 1);
+ DMatrixRMaj linearVelocity = new DMatrixRMaj(3, 1);
+ DMatrixRMaj gravityVector = new DMatrixRMaj(3, 1);
+ DMatrixRMaj rotationMatrix = new DMatrixRMaj(3, 3);
+ DMatrixRMaj acceleration = new DMatrixRMaj(3, 1);
+ DMatrixRMaj accelerationBias = new DMatrixRMaj(3, 1);
+ DMatrixRMaj accelerationUnbiased = new DMatrixRMaj(3, 1);
+ DMatrixRMaj accelerationUnbiasedInWorld = new DMatrixRMaj(3, 1);
+ DMatrixRMaj gyro = new DMatrixRMaj(3, 1);
+ DMatrixRMaj gyroBias = new DMatrixRMaj(3, 1);
+ DMatrixRMaj gyroUnbiased = new DMatrixRMaj(3, 1);
+
+ processState.translation.get(translation);
+ processState.linearVelocity.get(linearVelocity);
+ OdometryTools.toRotationMatrix(processState.orientation, rotationMatrix);
+ gravity.get(gravityVector);
+ sensedVariables.accelMeasurement.get(acceleration);
+ processState.accelBias.get(accelerationBias);
+ sensedVariables.gyroMeasurement.get(gyro);
+ processState.gyroBias.get(gyroBias);
+
+ DMatrixRMaj expectedPredictedTranslation = new DMatrixRMaj(3, 1);
+ DMatrixRMaj expectedPredictedLinearVelocity = new DMatrixRMaj(3, 1);
+ DMatrixRMaj expectedPredictedRotation = new DMatrixRMaj(4, 1);
+ DMatrixRMaj expectedPredictedAccelBias = new DMatrixRMaj(3, 1);
+ DMatrixRMaj expectedPredictedGyroBias = new DMatrixRMaj(3, 1);
+
+ // First term
+ CommonOps_DDRM.add(estimatorDt, linearVelocity, translation, expectedPredictedTranslation);
+
+ // Second term
+ if (OdometryKalmanFilter.includeBias)
+ CommonOps_DDRM.subtract(acceleration, accelerationBias, accelerationUnbiased);
+ else
+ accelerationUnbiased.set(acceleration);
+ CommonOps_DDRM.mult(rotationMatrix, accelerationUnbiased, accelerationUnbiasedInWorld);
+ CommonOps_DDRM.subtractEquals(accelerationUnbiasedInWorld, gravityVector);
+ CommonOps_DDRM.add(estimatorDt, accelerationUnbiasedInWorld, linearVelocity, expectedPredictedLinearVelocity);
+
+ // Third term
+ if (OdometryKalmanFilter.includeBias)
+ CommonOps_DDRM.subtract(gyro, gyroBias, gyroUnbiased);
+ else
+ gyroUnbiased.set(gyro);
+ CommonOps_DDRM.scale(estimatorDt, gyroUnbiased);
+ CommonOps_DDRM.mult(OdometryTools.lOperator(processState.orientation), OdometryTools.exponentialMap(gyroUnbiased), expectedPredictedRotation);
+
+ // Fourth term
+ expectedPredictedAccelBias.set(accelerationBias);
+
+ // Fifth term
+ expectedPredictedGyroBias.set(gyroBias);
+
+ DMatrixRMaj predictedTranslation = new DMatrixRMaj(3, 1);
+ DMatrixRMaj predictedLinearVelocity = new DMatrixRMaj(3, 1);
+ DMatrixRMaj predictedRotation = new DMatrixRMaj(4, 1);
+ DMatrixRMaj predictedAccelBias = new DMatrixRMaj(3, 1);
+ DMatrixRMaj predictedGyroBias = new DMatrixRMaj(3, 1);
+
+ predictedState.translation.get(predictedTranslation);
+ predictedState.linearVelocity.get(predictedLinearVelocity);
+ predictedRotation.set(OdomTestTools.toVector(predictedState.orientation));
+ predictedState.accelBias.get(predictedAccelBias);
+ predictedState.gyroBias.get(predictedGyroBias);
+
+ EjmlUnitTests.assertEquals(expectedPredictedTranslation, predictedTranslation, 1e-6);
+ EjmlUnitTests.assertEquals(expectedPredictedLinearVelocity, predictedLinearVelocity, 1e-6);
+ EjmlUnitTests.assertEquals(expectedPredictedRotation, predictedRotation, 1e-6);
+ EjmlUnitTests.assertEquals(expectedPredictedAccelBias, predictedAccelBias, 1e-6);
+ EjmlUnitTests.assertEquals(expectedPredictedGyroBias, predictedGyroBias, 1e-6);
+ }
+ }
+
+ @Test
+ public void testComputeProcessJacobian()
+ {
+ int iters = 100;
+ Random random = new Random(1738L);
+
+ YoRegistry test = new YoRegistry(getClass().getSimpleName());
+ TestIMU baseIMU = new TestIMU("base");
+ TestIMU footIMU = new TestIMU("foot");
+
+ StateVariables processState = new StateVariables("process", footIMU.getMeasurementFrame(), test);
+ StateVariables predictedState = new StateVariables("predicted", footIMU.getMeasurementFrame(), test);
+ SensedVariables sensedVariables = new SensedVariables("foot", baseIMU, footIMU, new TestBiasProvider(), test);
+
+ ProcessModel processModel = new ProcessModel(processState, predictedState, () -> true, gravity, estimatorDt);
+ DMatrixRMaj jacobian = new DMatrixRMaj(OdometryIndexHelper.errorSizePerLink, OdometryIndexHelper.errorSizePerLink);
+
+ for (int iter = 0; iter < iters; iter++)
+ {
+ jacobian.zero();
+ OdomTestTools.setRandomSensed(random, sensedVariables);
+ footIMU.linearAcceleration.set(sensedVariables.accelMeasurement);
+ footIMU.angularVelocity.set(sensedVariables.gyroMeasurement);
+ footIMU.pose.set(EuclidCoreRandomTools.nextPoint3D(random, 10.0), EuclidCoreRandomTools.nextOrientation3D(random));
+ footIMU.linearVelocity.set(EuclidCoreRandomTools.nextVector3D(random, 10.0));
+ footIMU.sensorFrame.update();
+ baseIMU.pose.set(EuclidCoreRandomTools.nextPoint3D(random, 10.0), EuclidCoreRandomTools.nextOrientation3D(random));
+ baseIMU.linearVelocity.set(EuclidCoreRandomTools.nextVector3D(random, 10.0));
+ baseIMU.sensorFrame.update();
+
+ OdomTestTools.setRandomState(random, sensedVariables, processState);
+ OdomTestTools.setRandomState(random, sensedVariables, predictedState);
+
+ processModel.computeProcessJacobian( 0, jacobian);
+
+ DMatrixRMaj jacobianExpected = new DMatrixRMaj(OdometryIndexHelper.errorSizePerLink, OdometryIndexHelper.errorSizePerLink);
+ // Row 1
+ CommonOps_DDRM.setIdentity(jacobianExpected);
+ for (int i = 0; i < 3; i++)
+ jacobianExpected.set(i, i + 3, estimatorDt);
+
+ // Row 2
+ DMatrixRMaj rotationMatrix = new DMatrixRMaj(3, 3);
+ DMatrixRMaj skewMatrix = new DMatrixRMaj(3, 3);
+ OdometryTools.toRotationMatrix(predictedState.orientation, rotationMatrix);
+ DMatrixRMaj acceleration = new DMatrixRMaj(3, 1);
+ DMatrixRMaj accelerationBias = new DMatrixRMaj(3, 1);
+ DMatrixRMaj accelerationUnbiased = new DMatrixRMaj(3, 1);
+ sensedVariables.accelMeasurement.get(acceleration);
+ predictedState.accelBias.get(accelerationBias);
+ if (OdometryKalmanFilter.includeBias)
+ CommonOps_DDRM.subtract(acceleration, accelerationBias, accelerationUnbiased);
+ else
+ accelerationUnbiased.set(acceleration);
+ CommonOps_DDRM.scale(estimatorDt, accelerationUnbiased);
+ OdometryTools.toSkewSymmetricMatrix(accelerationUnbiased.get(0), accelerationUnbiased.get(1), accelerationUnbiased.get(2), skewMatrix);
+ MatrixTools.multAddBlock(-1.0, rotationMatrix, skewMatrix, jacobianExpected, 3, 6);
+ if (OdometryKalmanFilter.includeBias)
+ MatrixTools.setMatrixBlock(jacobianExpected, 3, 9, rotationMatrix, 0, 0, 3, 3, -estimatorDt);
+
+ // Row 3
+ DMatrixRMaj velocity = new DMatrixRMaj(3, 1);
+ DMatrixRMaj velocityBias = new DMatrixRMaj(3, 1);
+ DMatrixRMaj velocityUnbiased = new DMatrixRMaj(3, 1);
+ sensedVariables.gyroMeasurement.get(velocity);
+ predictedState.gyroBias.get(velocityBias);
+ if (OdometryKalmanFilter.includeBias)
+ CommonOps_DDRM.subtract(velocity, velocityBias, velocityUnbiased);
+ else
+ velocityUnbiased.set(velocity);
+ CommonOps_DDRM.scale(estimatorDt, velocityUnbiased);
+ OdometryTools.toSkewSymmetricMatrix(velocityUnbiased.get(0), velocityUnbiased.get(1), velocityUnbiased.get(2), skewMatrix);
+ MatrixTools.addMatrixBlock(jacobianExpected, 6, 6, skewMatrix, 0, 0, 3, 3, -1.0);
+ if (OdometryKalmanFilter.includeBias)
+ {
+ for (int i = 0; i < 3; i++)
+ jacobianExpected.set(i + 6, i + 12, -estimatorDt);
+ }
+
+ // Row 4 and 5 are already set
+ EjmlUnitTests.assertEquals(jacobianExpected, jacobian, 1e-5);
+ }
+ }
+
+ static class TestBiasProvider implements IMUBiasProvider
+ {
+
+ @Override
+ public FrameVector3DReadOnly getAngularVelocityBiasInIMUFrame(IMUSensorReadOnly imu)
+ {
+ return new FrameVector3D(imu.getMeasurementFrame());
+ }
+
+ @Override
+ public FrameVector3DReadOnly getAngularVelocityBiasInWorldFrame(IMUSensorReadOnly imu)
+ {
+ return new FrameVector3D();
+ }
+
+ @Override
+ public FrameVector3DReadOnly getLinearAccelerationBiasInIMUFrame(IMUSensorReadOnly imu)
+ {
+ return new FrameVector3D(imu.getMeasurementFrame());
+ }
+
+ @Override
+ public FrameVector3DReadOnly getLinearAccelerationBiasInWorldFrame(IMUSensorReadOnly imu)
+ {
+ return new FrameVector3D();
+ }
+ }
+
+ static class TestIMU implements IMUSensorReadOnly
+ {
+ private String name;
+ public final MovingReferenceFrame sensorFrame;
+ public final Pose3D pose = new Pose3D();
+ public final Vector3D angularVelocity = new Vector3D();
+ public final Vector3D linearVelocity = new Vector3D();
+ public final Vector3D linearAcceleration = new Vector3D();
+
+ public TestIMU(String name)
+ {
+ this.name = name;
+
+ sensorFrame = new MovingReferenceFrame(name, ReferenceFrame.getWorldFrame())
+ {
+ @Override
+ protected void updateTwistRelativeToParent(Twist twist)
+ {
+ twist.setToZero();
+ pose.transform(linearVelocity, twist.getLinearPart());
+ }
+
+ @Override
+ protected void updateTransformToParent(RigidBodyTransform rigidBodyTransform)
+ {
+ rigidBodyTransform.set(pose);
+ }
+ };
+ }
+
+ @Override
+ public String getSensorName()
+ {
+ return name;
+ }
+
+ @Override
+ public MovingReferenceFrame getMeasurementFrame()
+ {
+ return sensorFrame;
+ }
+
+ @Override
+ public RigidBodyBasics getMeasurementLink()
+ {
+ return null;
+ }
+
+ @Override
+ public QuaternionReadOnly getOrientationMeasurement()
+ {
+ return pose.getOrientation();
+ }
+
+ @Override
+ public Vector3DReadOnly getAngularVelocityMeasurement()
+ {
+ return angularVelocity;
+ }
+
+ @Override
+ public Vector3DReadOnly getLinearAccelerationMeasurement()
+ {
+ return linearAcceleration;
+ }
+
+ @Override
+ public void getOrientationNoiseCovariance(DMatrixRMaj noiseCovarianceToPack)
+ {
+
+ }
+
+ @Override
+ public void getAngularVelocityNoiseCovariance(DMatrixRMaj noiseCovarianceToPack)
+ {
+
+ }
+
+ @Override
+ public void getAngularVelocityBiasProcessNoiseCovariance(DMatrixRMaj biasProcessNoiseCovarianceToPack)
+ {
+
+ }
+
+ @Override
+ public void getLinearAccelerationNoiseCovariance(DMatrixRMaj noiseCovarianceToPack)
+ {
+
+ }
+
+ @Override
+ public void getLinearAccelerationBiasProcessNoiseCovariance(DMatrixRMaj biasProcessNoiseCovarianceToPack)
+ {
+
+ }
+ }
+}