From ee1ef8c9bf091303c060c8353c536b8ff947592a Mon Sep 17 00:00:00 2001 From: Robert Date: Wed, 27 Aug 2025 10:30:13 -0500 Subject: [PATCH 01/18] did a bit of a refactor on the EKF and an improved javadoc --- .../ExtendedKalmanFilter.java | 16 ++ ...ExampleConstantVelocity2DKalmanFilter.java | 74 +++++++++ .../ExamplePendulumExtendedKalmanFilter.java | 136 +++++++++++++++++ ...antVelocity2DExtendedKalmanFilterDemo.java | 88 +---------- .../PendulumExtendedKalmanFilterDemo.java | 141 ++++-------------- 5 files changed, 266 insertions(+), 189 deletions(-) create mode 100644 ihmc-parameter-estimation/src/main/java/us/ihmc/parameterEstimation/examples/ExampleConstantVelocity2DKalmanFilter.java create mode 100644 ihmc-parameter-estimation/src/main/java/us/ihmc/parameterEstimation/examples/ExamplePendulumExtendedKalmanFilter.java 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..83f357673a45 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 @@ -13,6 +13,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: + *

+ * 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: + *

+ *

* * @author James Foster */ 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]; From b37517df6dbc4d8e2a6f53d2184a6afeabdbc04d Mon Sep 17 00:00:00 2001 From: Robert Date: Wed, 27 Aug 2025 17:05:26 -0500 Subject: [PATCH 02/18] started adding an odometry extended kalman filter --- .../ExtendedKalmanFilter.java | 23 +- .../DRCKinematicsBasedStateEstimator.java | 11 + .../odomEKF/OdometryKalmanFilter.java | 573 ++++++++++++++++++ .../odomEKF/OdometryKalmanFilterTest.java | 57 ++ 4 files changed, 663 insertions(+), 1 deletion(-) create mode 100644 ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryKalmanFilter.java create mode 100644 ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryKalmanFilterTest.java 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 83f357673a45..f6a7d070520c 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 @@ -26,7 +26,7 @@ * For example implementations, see: *
    *
  • {@link us.ihmc.parameterEstimation.examples.ExamplePendulumExtendedKalmanFilter}
    • - *
  • {@link us.ihmc.parameterEstimation.examples.ExamplePendulumExtendedKalmanFilter}
    • + *
  • {@link us.ihmc.parameterEstimation.examples.ExampleConstantVelocity2DKalmanFilter}
    • *
*

* @@ -144,8 +144,14 @@ public abstract class ExtendedKalmanFilter private final DMatrixRMaj josephGainTerm; private final DMatrixRMaj josephGainTermContainer; + private final int stateSize; + private final int measurementSize; + public ExtendedKalmanFilter(int stateSize, int measurementSize) { + this.stateSize = stateSize; + this.measurementSize = measurementSize; + processCovariance = new DMatrixRMaj(stateSize, stateSize); measurementCovariance = new DMatrixRMaj(measurementSize, measurementSize); @@ -204,6 +210,16 @@ public ExtendedKalmanFilter(DMatrixRMaj initialState, DMatrixRMaj initialCovaria covariance.set(initialCovariance); } + public int getStateSize() + { + return stateSize; + } + + public int getMeasurementSize() + { + return measurementSize; + } + /** * Runs the filter for a single time step. * @@ -440,6 +456,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-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..68792aaba966 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,9 @@ public void doControl() for (int i = 0; i < footSwitchList.size(); i++) footSwitchList.get(i).update(); + // TODO should this also consume some of the foot switch data? + odometry.compute(); + switch (operatingMode.getEnumValue()) { case FROZEN: 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..d6449feae4f0 --- /dev/null +++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryKalmanFilter.java @@ -0,0 +1,573 @@ +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.FrameVector3DBasics; +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.tuple3D.interfaces.Vector3DReadOnly; +import us.ihmc.euclid.tuple4D.Quaternion; +import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; +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 java.util.ArrayList; +import java.util.List; + +/** + * The process model in this class is defined as + */ +public class OdometryKalmanFilter extends ExtendedKalmanFilter +{ + // Constants and providers + private final BooleanProvider cancelGravityFromAccelerationMeasurement; + + 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 double estimatorDT; + private final FrameVector3DReadOnly gravityVector; + + // Internal variables for state holding + private final YoFrameVector3D yoBaseLinearAccelerationMeasurementInWorld; + private final YoFrameVector3D yoBaseLinearAccelerationMeasurement; + private final YoFrameVector3D yoBaseAngularVelocityMeasurement; + + private final YoFrameVector3D yoRootJointAngularVelocityMeasurementInWorld; + private final YoFrameVector3D yoRootJointAngularVelocityMeasurement; + + private final List yoFootLinearAccelerationMeasurementInWorld = new ArrayList<>(); + private final List yoFootLinearAccelerationMeasurement = new ArrayList<>(); + + // Outputs + private final DMatrixRMaj predictedState; + private final DMatrixRMaj predictedMeasurement; + + private final DMatrixRMaj AMatrix; + private final DMatrixRMaj CMatrix; + + private final YoFrameQuaternion predictedBaseOrientation; + private final YoFramePoint3D predictedBaseTranslation; + private final YoFrameVector3D predictedBaseLinearVelocity; + + 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 List predictedFootTranslations = new ArrayList<>(); + private final List predictedFootLinearVelocities = new ArrayList<>(); + + private final RigidBodyTransformReadOnly transformToRootJoint; + + private final List predictedRelativeFootPositionMeasurements = new ArrayList<>(); + private final List predictedRelativeFootVelocityMeasurements = new ArrayList<>(); + private final List predictedWorldFootVelocityMeasurements = new ArrayList<>(); + + // Temporary variables + private final FrameVector3D linearAcceleration = new FrameVector3D(); + private final FrameVector3D linearVelocity = new FrameVector3D(); + private final Vector3D deltaVector = new Vector3D(); + private final Quaternion estimatedRootJointRotation = new Quaternion(); + private final DMatrixRMaj rotation3x3 = new DMatrixRMaj(3, 3); + + private final DMatrixRMaj predictedBaseIMUPositionState = new DMatrixRMaj(3, 1); + private final DMatrixRMaj predictedBaseIMUVelocityState = new DMatrixRMaj(3, 1); + private final DMatrixRMaj predictedFootPositionState = new DMatrixRMaj(3, 1); + private final DMatrixRMaj predictedFootVelocityState = new DMatrixRMaj(3, 1); + + private final DMatrixRMaj offsetVector = new DMatrixRMaj(3, 1); + private final DMatrixRMaj footCovariance = new DMatrixRMaj(3, 3); + + private final DMatrixRMaj eye3x3 = CommonOps_DDRM.identity(3); + private final Twist twist = new Twist(); + + public OdometryKalmanFilter(IMUSensorReadOnly baseIMU, + List feetIMUs, + IMUBiasProvider baseImuBiasProvider, + BooleanProvider cancelGravityFromAccelerationMeasurement, + double estimatorDT, + double gravitationalAcceleration, + YoRegistry parentRegistry) + { + super(10 + feetIMUs.size() * 6, 9 * feetIMUs.size()); + + this.baseIMU = baseIMU; + this.feetIMUs = feetIMUs; + this.imuBiasProvider = baseImuBiasProvider; + this.cancelGravityFromAccelerationMeasurement = cancelGravityFromAccelerationMeasurement; + this.estimatorDT = estimatorDT; + + predictedState = new DMatrixRMaj(getStateSize(), 1); + predictedMeasurement = new DMatrixRMaj(getMeasurementSize(), 1); + AMatrix = new DMatrixRMaj(getStateSize(), getStateSize()); + CMatrix = new DMatrixRMaj(getMeasurementSize(), getStateSize()); + + gravityVector = EuclidFrameFactories.newLinkedFrameVector3DReadOnly(() -> worldFrame, new Vector3D(0, 0, -Math.abs(gravitationalAcceleration))); + + predictedBaseOrientation = new YoFrameQuaternion("predictedBaseOrientation", worldFrame, registry); + predictedBaseTranslation = new YoFramePoint3D("predictedBaseTranslation", worldFrame, registry); + predictedBaseLinearVelocity = new YoFrameVector3D("predictedBaseLinearVelocity", worldFrame, 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()); + + yoBaseLinearAccelerationMeasurement = new YoFrameVector3D("baseImuLinearAcceleration", baseIMU.getMeasurementFrame(), registry); + yoBaseLinearAccelerationMeasurementInWorld = new YoFrameVector3D("baseImuLinearAccelerationInWorld", worldFrame, registry); + yoBaseAngularVelocityMeasurement = new YoFrameVector3D("baseImuLinearAccelerationInWorld", baseIMU.getMeasurementFrame(), registry); + yoRootJointAngularVelocityMeasurement = new YoFrameVector3D("rootJointImuLinearAccelerationInWorld", baseIMU.getMeasurementFrame(), registry); + yoRootJointAngularVelocityMeasurementInWorld = new YoFrameVector3D("rootJointImuLinearAccelerationInWorld", worldFrame, registry); + + for (int i = 0; i < feetIMUs.size(); i++) + { + IMUSensorReadOnly footIMU = feetIMUs.get(i); + YoFrameVector3D footLinearAccelerationMeasurement = new YoFrameVector3D("foot" + i + "ImuLinearAcceleration", footIMU.getMeasurementFrame(), registry); + YoFrameVector3D footLinearAccelerationMeasurementInWorld = new YoFrameVector3D("foot" + i + "ImuLinearAccelerationInWorld", worldFrame, registry); + yoFootLinearAccelerationMeasurement.add(footLinearAccelerationMeasurement); + yoFootLinearAccelerationMeasurementInWorld.add(footLinearAccelerationMeasurementInWorld); + + predictedFootTranslations.add(new YoFramePoint3D("predictedFoot" + i + "Translation", worldFrame, registry)); + predictedFootLinearVelocities.add(new YoFrameVector3D("predictedFoot" + i + "LinearVelocity", worldFrame, registry)); + + predictedRelativeFootPositionMeasurements.add(new YoFramePoint3D("predictedRelativeFoot" + i + "PositionMeasurement", baseIMU.getMeasurementFrame(), registry)); + predictedRelativeFootVelocityMeasurements.add(new YoFrameVector3D("predictedRelativeFoot" + i + "VelocityMeasurement", baseIMU.getMeasurementFrame(), registry)); + predictedWorldFootVelocityMeasurements.add(new YoFrameVector3D("predictedWorldFoot" + i + "VelocityMeasurement", worldFrame, registry)); + } + + parentRegistry.addChild(registry); + } + + public void compute() + { + updateMeasures(); + updateRootJointTwistAngularPart(); + + // FIXME populate the observations, which are + // FIXME 1. the relative distance between the IMU and the foot in the IMU frame + // FIXME 2. the relative linear velocity between the IMU and the foot in the IMU frame + // FIXME 3. the absolute linear velocity of the foot + + DMatrixRMaj stateEstimate = calculateEstimate(); + + 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 final FramePose3D tempPose = new FramePose3D(); + + + public void updateMeasures() + { + // TODO re-enable this +// if (!isEstimationEnabled()) +// return; + + FrameVector3DReadOnly biasInput = imuBiasProvider.getLinearAccelerationBiasInIMUFrame(baseIMU); + Vector3DReadOnly rawInput = baseIMU.getLinearAccelerationMeasurement(); + + linearAcceleration.setReferenceFrame(baseIMU.getMeasurementFrame()); + linearAcceleration.sub(rawInput, biasInput); + + // Update acceleration in world (minus gravity) + if (cancelGravityFromAccelerationMeasurement.getValue()) + { + // FIXME use the estimated orientation of the base instead of the measured one. + linearAcceleration.changeFrame(worldFrame); + linearAcceleration.add(gravityVector); + } + + yoBaseLinearAccelerationMeasurementInWorld.setMatchingFrame(linearAcceleration); + yoBaseLinearAccelerationMeasurement.setMatchingFrame(linearAcceleration); + + for (int i = 0; i < feetIMUs.size(); i++) + { + IMUSensorReadOnly footIMU = feetIMUs.get(i); + FrameVector3DReadOnly footBiasInput = imuBiasProvider.getLinearAccelerationBiasInIMUFrame(footIMU); + Vector3DReadOnly footRawInput = footIMU.getLinearAccelerationMeasurement(); + + linearAcceleration.setReferenceFrame(footIMU.getMeasurementFrame()); + linearAcceleration.sub(footRawInput, footBiasInput); + + // Update acceleration in world (minus gravity) + if (cancelGravityFromAccelerationMeasurement.getValue()) + { + // FIXME use the estimated orientation of the base instead of the measured one. + linearAcceleration.changeFrame(worldFrame); + linearAcceleration.add(gravityVector); + } + + yoFootLinearAccelerationMeasurementInWorld.get(i).setMatchingFrame(linearAcceleration); + yoFootLinearAccelerationMeasurement.get(i).setMatchingFrame(linearAcceleration); + } + } + + 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 + public DMatrixRMaj processModel(DMatrixRMaj state) + { + predictedState.set(state); + estimatedRootJointRotation.set(6, state); + + // Compute the predicted position state + // p_k+1 = p_k + v_k * dt + MatrixTools.setMatrixBlock(predictedBaseIMUPositionState, 0, 0, state, 0, 0, 3, 1, 1.0); + MatrixTools.addMatrixBlock(predictedBaseIMUPositionState, 0, 0, state, 3, 0, 3, 1, estimatorDT ); + predictedBaseTranslation.set(predictedBaseIMUPositionState); + + // Compute the predicted velocity state + // v_k+1 = v_k + a_k * dt + // get the acceleration of the base IMU in the base frame, but using the estimated orientation that is internal to the filter. + linearAcceleration.setIncludingFrame(yoBaseLinearAccelerationMeasurement); + estimatedRootJointRotation.transform(linearAcceleration); // TODO make sure this isn't an inverse transform + // figure out the change in velocity based on integrating this acceleration + linearAcceleration.get(predictedBaseIMUVelocityState); + CommonOps_DDRM.scale(estimatorDT, predictedBaseIMUVelocityState); + // add to the velocity the current velocity estimate + MatrixTools.addMatrixBlock(predictedBaseIMUVelocityState, 0, 0, state, 3, 0, 3, 1, 1.0); + predictedBaseLinearVelocity.set(predictedBaseIMUVelocityState); + + // Compute the predicted orientation state by integrating the angular velocity measurement + deltaVector.setAndScale(estimatorDT, baseIMU.getAngularVelocityMeasurement()); + estimatedRootJointRotation.transform(deltaVector); + predictedBaseOrientation.set(estimatedRootJointRotation); + predictedBaseOrientation.append(estimatedRootJointRotation); + + // Pack everything into the predicted state vector + predictedBaseTranslation.get(predictedState); + predictedBaseLinearVelocity.get(3, predictedState); + predictedBaseOrientation.get(6, predictedState); + + // Compute the predicted foot position and velocity states + for (int i = 0; i < feetIMUs.size(); i++) + { + int offset = 10 + i * 6; + // Compute the predicted foot position state + // p_k+1 = p_k + v_k * dt + MatrixTools.setMatrixBlock(predictedFootPositionState, 0, 0, state, offset, 0, 3 , 1, 1.0); + MatrixTools.addMatrixBlock(predictedFootPositionState, 0, 0, state, offset + 3, 0, 3, 1, estimatorDT); + predictedFootTranslations.get(i).set(predictedFootPositionState); + + // Compute the predicted foot velocity state + // v_k+1 = v_k + a_k * dt + // First, get the acceleration of the foot IMU in the base IMU frame, which we're considering here as the root. + linearAcceleration.setIncludingFrame(yoFootLinearAccelerationMeasurement.get(i)); + linearAcceleration.changeFrame(baseIMU.getMeasurementFrame()); + // Now, get the acceleration in the estimated world frame + estimatedRootJointRotation.transform(linearAcceleration); // TODO make sure this isn't an inverse transform + linearAcceleration.get(predictedFootVelocityState); + CommonOps_DDRM.scale(estimatorDT, predictedFootVelocityState); + // add to the velocity the current velocity estimate + MatrixTools.addMatrixBlock(predictedFootVelocityState, 0, 0, state, offset + 3, 0, 3, 1, 1.0); + predictedFootLinearVelocities.get(i).set(predictedFootVelocityState); + + // Pack into the prediction vector. + predictedFootTranslations.get(i).get(offset, predictedState); + predictedFootLinearVelocities.get(i).get(offset + 3, predictedState); + } + + // return the predicted state + return predictedState; + } + + @Override + public DMatrixRMaj measurementModel(DMatrixRMaj state) + { + estimatedRootJointRotation.set(6, state); + + for (int i = 0; i < feetIMUs.size(); i++) + { + int measurementOffset = 9 * i; + int footStateOffset = 10 + i * 6; + + // Compute the predicted foot relative position measurement + MatrixTools.setMatrixBlock(offsetVector, 0, 0, state, 0, 0, 3 , 1, 1.0); + MatrixTools.addMatrixBlock(offsetVector, 0, 0, state, footStateOffset, 0, 3 , 1, 1.0); + + predictedRelativeFootPositionMeasurements.get(i).set(offsetVector); + estimatedRootJointRotation.inverseTransform(predictedRelativeFootPositionMeasurements.get(i)); // TODO make sure this isn't a transform + + // Compute the predicted foot relative velocity measurement + MatrixTools.setMatrixBlock(offsetVector, 0, 0, state, 3, 0, 3 , 1, 1.0); + MatrixTools.addMatrixBlock(offsetVector, 0, 0, state, footStateOffset + 3, 0, 3 , 1, 1.0); + + predictedRelativeFootVelocityMeasurements.get(i).set(offsetVector); + estimatedRootJointRotation.inverseTransform(predictedRelativeFootVelocityMeasurements.get(i)); // TODO make sure this isn't a transform + + // If the foot is in contact, set the velocity to zero, otherwise use the previous prediction state + if (isFootInContact(i, state)) + { + predictedWorldFootVelocityMeasurements.get(i).setToZero(); + } + else + { + predictedWorldFootVelocityMeasurements.get(i).set(footStateOffset + 3, state); + } + + predictedRelativeFootPositionMeasurements.get(i).get(measurementOffset, predictedMeasurement); + predictedRelativeFootVelocityMeasurements.get(i).get(measurementOffset + 3, predictedMeasurement); + predictedWorldFootVelocityMeasurements.get(i).get(measurementOffset + 6, predictedMeasurement); + } + + // return the predicted measurement + return predictedMeasurement; + } + + private final DMatrixRMaj temp = new DMatrixRMaj(3, 1); + private final DMatrixRMaj tempScalar = new DMatrixRMaj(1, 1); + private boolean isFootInContact(int footIndex, DMatrixRMaj state) + { + int startRow = 10 + footIndex * 6; + CommonOps_DDRM.extract(state, startRow, startRow + 3, 0, 1, offsetVector, 0, 0); + CommonOps_DDRM.extract(getCovariance(), startRow, startRow + 3, startRow, startRow + 3, footCovariance, 0, 0); + + CommonOps_DDRM.mult(footCovariance, offsetVector, temp); + CommonOps_DDRM.multTransA(offsetVector, temp, tempScalar); + + // tests the mahalonobis distance of the foot velocity being below a certain threshold. + return tempScalar.get(0, 0) < 0.2; + } + + @Override + protected DMatrixRMaj linearizeProcessModel(DMatrixRMaj previousState) + { + // Populate the gradient of the base position w.r.t. the state + CommonOps_DDRM.insert(eye3x3, AMatrix, 0, 0); + MatrixTools.setMatrixBlock(AMatrix, 0, 3, eye3x3, 0, 0, 3, 3, estimatorDT); + + // Populate the gradient of the base velocity w.r.t. the state + CommonOps_DDRM.insert(eye3x3, AMatrix, 3, 3); + // FIXME add the effect of the gradient of the base rotation + + // FIXME do the gradient crap for the base orientation, which is going to be frustrating + + for (int i = 0; i < feetIMUs.size(); i++) + { + int stateOffset = 10 + i * 6; + // Populate the gradient of the foot position w.r.t. the state + CommonOps_DDRM.insert(eye3x3, AMatrix, stateOffset, stateOffset); + MatrixTools.setMatrixBlock(AMatrix, stateOffset, stateOffset + 3, eye3x3, 0, 0, 3, 3, estimatorDT); + + // Populate the gradient of the foot position w.r.t. the state + // FIXME add the effect of the gradient of the base rotation + CommonOps_DDRM.insert(eye3x3, AMatrix, stateOffset + 3, stateOffset + 3); + } + + return AMatrix; + } + + @Override + protected DMatrixRMaj linearizeMeasurementModel(DMatrixRMaj predictedState) + { + estimatedRootJointRotation.set(6, predictedState); + toRotationMatrix(estimatedRootJointRotation, rotation3x3); + + CommonOps_DDRM.transpose(rotation3x3); + + for (int i = 0; i < feetIMUs.size(); i++) + { + int footStateOffset = 10 + i * 6; + int measureOffset = 9 * i; + // R^T * (s_k - p_k) + CommonOps_DDRM.insert(rotation3x3, CMatrix, measureOffset, footStateOffset); + MatrixTools.setMatrixBlock(CMatrix, measureOffset, 0, rotation3x3, 0, 0, 3, 3, -1.0); + + MatrixTools.setMatrixBlock(offsetVector, 0, 0, predictedState, footStateOffset, 0, 3, 1, 1.0); + MatrixTools.addMatrixBlock(offsetVector, 0, 0, predictedState, 0, 0, 3, 1, -1.0); + + // TODO add the effect of the gradient of the base rotation + + // R^T * (v_k - sDot_k) + CommonOps_DDRM.insert(rotation3x3, CMatrix, measureOffset + 3, 3); + MatrixTools.setMatrixBlock(CMatrix, measureOffset + 3, footStateOffset + 3, rotation3x3, 0, 0, 3, 3, -1.0); + // TODO add the effect of the gradient of the base rotation + + MatrixTools.setMatrixBlock(offsetVector, 0, 0, predictedState, 3, 0, 3, 1, 1.0); + MatrixTools.addMatrixBlock(offsetVector, 0, 0, predictedState, footStateOffset + 3, 0, 3, 1, -1.0); + + // sDot_k + CommonOps_DDRM.insert(eye3x3, CMatrix, measureOffset + 6, footStateOffset + 3); + } + + return CMatrix; + } + + static void toRotationMatrix(QuaternionReadOnly quaternion, DMatrixRMaj rotationMatrix) + { + toRotationMatrix(quaternion.getX(), quaternion.getY(), quaternion.getZ(), quaternion.getS(), rotationMatrix); + } + + 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 toRotationMatrixInverse(QuaternionReadOnly quaternion, DMatrixRMaj rotationMatrix) + { + toRotationMatrixInverse(quaternion.getX(), quaternion.getY(), quaternion.getZ(), quaternion.getS(), rotationMatrix); + } + + 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)); + } +} diff --git a/ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryKalmanFilterTest.java b/ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryKalmanFilterTest.java new file mode 100644 index 000000000000..a53fb87195d4 --- /dev/null +++ b/ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryKalmanFilterTest.java @@ -0,0 +1,57 @@ +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.tuple4D.Quaternion; + +import java.util.Random; + +public class OdometryKalmanFilterTest +{ + @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); + + OdometryKalmanFilter.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); + + OdometryKalmanFilter.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); + } + } +} From 55ffcf0fa2dd69570324b15631cfe3f7291278f4 Mon Sep 17 00:00:00 2001 From: Robert Date: Thu, 28 Aug 2025 13:56:57 -0500 Subject: [PATCH 03/18] updated the plant model --- .../odomEKF/OdometryIndexHelper.java | 126 +++++ .../odomEKF/OdometryKalmanFilter.java | 429 +++++++++++------- 2 files changed, 391 insertions(+), 164 deletions(-) create mode 100644 ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryIndexHelper.java 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..254661f746a3 --- /dev/null +++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryIndexHelper.java @@ -0,0 +1,126 @@ +package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF; + +import us.ihmc.euclid.tools.EuclidCoreTools; +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; + +public class OdometryIndexHelper +{ + public static int getStateSizePerLink() + { + return 16; + } + + public static int getStatePositionIndex() + { + return 0; + } + + public static int getStateVelocityIndex() + { + return 3; + } + + public static int getStateOrientationIndex() + { + return 6; + } + + public static int getStateAccelerationBiasIndex() + { + return 10; + } + + public static int getStateGyroBiasIndex() + { + return 13; + } + + public static int getBasePositionIndex() + { + return getStatePositionIndex(); + } + + public static int getBaseVelocityIndex() + { + return getStateVelocityIndex(); + } + + public static int getBaseOrientationIndex() + { + return getStateOrientationIndex(); + } + + public static int getBaseAccelerationBiasIndex() + { + return getStateAccelerationBiasIndex(); + } + + public static int getBaseGyroBiasIndex() + { + return getStateGyroBiasIndex(); + } + + public static int getFootPositionIndex(int footNumber) + { + return getStateSizePerLink() * (footNumber + 1) + getStatePositionIndex(); + } + + public static int getFootVelocityIndex(int footNumber) + { + return getFootPositionIndex(footNumber) + getStateVelocityIndex(); + } + + public static int getFootOrientationIndex(int footNumber) + { + return getFootPositionIndex(footNumber) + getStateOrientationIndex(); + } + + public static int getFootAccelerationBiasIndex(int footNumber) + { + return getFootPositionIndex(footNumber) + getStateAccelerationBiasIndex(); + } + + public static int getFootGyroBiasIndex(int footNumber) + { + return getFootPositionIndex(footNumber) + getStateGyroBiasIndex(); + } + + public static void toQuaternionFromRotationVector(Vector3DReadOnly rotation, QuaternionBasics quaternionToPack) + { + double magnitude = rotation.norm(); + double s = Math.sin(magnitude * 0.5); + double qs = Math.cos(magnitude * 0.5); + 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 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) + { + 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); + } +} 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 index d6449feae4f0..050c4b9c0ab2 100644 --- 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 @@ -7,7 +7,6 @@ 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.FrameVector3DBasics; import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; import us.ihmc.euclid.referenceFrame.tools.EuclidFrameFactories; import us.ihmc.euclid.transform.interfaces.RigidBodyTransformReadOnly; @@ -22,6 +21,8 @@ import us.ihmc.parameterEstimation.ExtendedKalmanFilter; import us.ihmc.sensorProcessing.stateEstimation.IMUSensorReadOnly; import us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.IMUBiasProvider; +import us.ihmc.yoVariables.euclid.YoQuaternion; +import us.ihmc.yoVariables.euclid.YoVector3D; import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D; import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePose3D; import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameQuaternion; @@ -29,6 +30,7 @@ import us.ihmc.yoVariables.providers.BooleanProvider; import us.ihmc.yoVariables.registry.YoRegistry; +import java.awt.*; import java.util.ArrayList; import java.util.List; @@ -38,8 +40,6 @@ public class OdometryKalmanFilter extends ExtendedKalmanFilter { // Constants and providers - private final BooleanProvider cancelGravityFromAccelerationMeasurement; - private static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); private final YoRegistry registry = new YoRegistry(getClass().getSimpleName()); @@ -52,16 +52,11 @@ public class OdometryKalmanFilter extends ExtendedKalmanFilter private final FrameVector3DReadOnly gravityVector; // Internal variables for state holding - private final YoFrameVector3D yoBaseLinearAccelerationMeasurementInWorld; - private final YoFrameVector3D yoBaseLinearAccelerationMeasurement; private final YoFrameVector3D yoBaseAngularVelocityMeasurement; private final YoFrameVector3D yoRootJointAngularVelocityMeasurementInWorld; private final YoFrameVector3D yoRootJointAngularVelocityMeasurement; - private final List yoFootLinearAccelerationMeasurementInWorld = new ArrayList<>(); - private final List yoFootLinearAccelerationMeasurement = new ArrayList<>(); - // Outputs private final DMatrixRMaj predictedState; private final DMatrixRMaj predictedMeasurement; @@ -69,9 +64,19 @@ public class OdometryKalmanFilter extends ExtendedKalmanFilter private final DMatrixRMaj AMatrix; private final DMatrixRMaj CMatrix; - private final YoFrameQuaternion predictedBaseOrientation; - private final YoFramePoint3D predictedBaseTranslation; - private final YoFrameVector3D predictedBaseLinearVelocity; + private final MeasuredVariables baseMeasurement; + private final List feetMeasurements = new ArrayList<>(); + + private final EstimatedVariables baseCurrentState; + private final EstimatedVariables basePredictedState; + private final List feetCurrentState = new ArrayList<>(); + private final List feetPredictedState = new ArrayList<>(); + + private final List sensorProcess = new ArrayList<>(); + private final List sensorMeasurements = new ArrayList<>(); + + private final EstimatedVariables baseObserveState; + private final List feetObservationState = new ArrayList<>(); private final YoFrameQuaternion filteredBaseOrientation; private final YoFramePoint3D filteredBaseTranslation; @@ -81,14 +86,8 @@ public class OdometryKalmanFilter extends ExtendedKalmanFilter private final YoFrameVector3D estimatedRootJointLinearVelocity; private final YoFrameVector3D estimatedRootJointAngularVelocity; - private final List predictedFootTranslations = new ArrayList<>(); - private final List predictedFootLinearVelocities = new ArrayList<>(); - private final RigidBodyTransformReadOnly transformToRootJoint; - private final List predictedRelativeFootPositionMeasurements = new ArrayList<>(); - private final List predictedRelativeFootVelocityMeasurements = new ArrayList<>(); - private final List predictedWorldFootVelocityMeasurements = new ArrayList<>(); // Temporary variables private final FrameVector3D linearAcceleration = new FrameVector3D(); @@ -97,11 +96,6 @@ public class OdometryKalmanFilter extends ExtendedKalmanFilter private final Quaternion estimatedRootJointRotation = new Quaternion(); private final DMatrixRMaj rotation3x3 = new DMatrixRMaj(3, 3); - private final DMatrixRMaj predictedBaseIMUPositionState = new DMatrixRMaj(3, 1); - private final DMatrixRMaj predictedBaseIMUVelocityState = new DMatrixRMaj(3, 1); - private final DMatrixRMaj predictedFootPositionState = new DMatrixRMaj(3, 1); - private final DMatrixRMaj predictedFootVelocityState = new DMatrixRMaj(3, 1); - private final DMatrixRMaj offsetVector = new DMatrixRMaj(3, 1); private final DMatrixRMaj footCovariance = new DMatrixRMaj(3, 3); @@ -131,9 +125,24 @@ public OdometryKalmanFilter(IMUSensorReadOnly baseIMU, gravityVector = EuclidFrameFactories.newLinkedFrameVector3DReadOnly(() -> worldFrame, new Vector3D(0, 0, -Math.abs(gravitationalAcceleration))); - predictedBaseOrientation = new YoFrameQuaternion("predictedBaseOrientation", worldFrame, registry); - predictedBaseTranslation = new YoFramePoint3D("predictedBaseTranslation", worldFrame, registry); - predictedBaseLinearVelocity = new YoFrameVector3D("predictedBaseLinearVelocity", worldFrame, registry); + baseMeasurement = new MeasuredVariables("base", baseIMU, imuBiasProvider, registry); + baseCurrentState = new EstimatedVariables("baseCurrent"); + baseObserveState = new EstimatedVariables("baseObserve"); + basePredictedState = new EstimatedVariables("basePredicted"); + sensorProcess.add(new SensorProcess(baseMeasurement, baseCurrentState, basePredictedState, cancelGravityFromAccelerationMeasurement, gravityVector, estimatorDT)); + for (int i = 0; i < feetIMUs.size(); i++) + { + MeasuredVariables footMeasurements = new MeasuredVariables("foot" + i, feetIMUs.get(i), imuBiasProvider, registry); + EstimatedVariables footCurrentState = new EstimatedVariables("foot" + i + "Current"); + EstimatedVariables footObservationState = new EstimatedVariables("foot" + i + "Observe"); + EstimatedVariables footPredictedState = new EstimatedVariables("foot" + i + "Predicted"); + feetMeasurements.add(footMeasurements); + feetObservationState.add(footObservationState); + feetCurrentState.add(footCurrentState); + feetPredictedState.add(footPredictedState); + sensorProcess.add(new SensorProcess(footMeasurements, footCurrentState, footPredictedState, cancelGravityFromAccelerationMeasurement, gravityVector, estimatorDT)); + sensorMeasurements.add(new SensorMeasurement("foot" + i, baseObserveState, footObservationState, baseIMU, feetIMUs.get(i), registry)); + } filteredBaseOrientation = new YoFrameQuaternion("filteredBaseOrientation", worldFrame, registry); filteredBaseTranslation = new YoFramePoint3D("filteredBaseTranslation", worldFrame, registry); @@ -145,29 +154,218 @@ public OdometryKalmanFilter(IMUSensorReadOnly baseIMU, transformToRootJoint = baseIMU.getMeasurementFrame().getTransformToDesiredFrame(baseIMU.getMeasurementLink().getParentJoint().getFrameAfterJoint()); - yoBaseLinearAccelerationMeasurement = new YoFrameVector3D("baseImuLinearAcceleration", baseIMU.getMeasurementFrame(), registry); - yoBaseLinearAccelerationMeasurementInWorld = new YoFrameVector3D("baseImuLinearAccelerationInWorld", worldFrame, registry); yoBaseAngularVelocityMeasurement = new YoFrameVector3D("baseImuLinearAccelerationInWorld", baseIMU.getMeasurementFrame(), registry); yoRootJointAngularVelocityMeasurement = new YoFrameVector3D("rootJointImuLinearAccelerationInWorld", baseIMU.getMeasurementFrame(), registry); yoRootJointAngularVelocityMeasurementInWorld = new YoFrameVector3D("rootJointImuLinearAccelerationInWorld", worldFrame, registry); - for (int i = 0; i < feetIMUs.size(); i++) + parentRegistry.addChild(registry); + } + + private static class SensorProcess + { + // State providers + private final MeasuredVariables measuredVariables; + private final EstimatedVariables currentState; + private final EstimatedVariables predictedState; + private final BooleanProvider cancelGravityFromAccelerationMeasurement; + private final FrameVector3DReadOnly gravityVector; + private final double estimatorDt; + + // Temp variables + private final Vector3D unbiasedAcceleration = new Vector3D(); + private final Vector3D unbiasedAngularVelocity = new Vector3D(); + private final Vector3D integratedVelocity = new Vector3D(); + private final Quaternion integratedRotation = new Quaternion(); + + public SensorProcess(MeasuredVariables measuredVariables, + EstimatedVariables currentState, + EstimatedVariables predictedState, + BooleanProvider cancelGravityFromAccelerationMeasurement, + FrameVector3DReadOnly gravityVector, + double estimatorDt) { - IMUSensorReadOnly footIMU = feetIMUs.get(i); - YoFrameVector3D footLinearAccelerationMeasurement = new YoFrameVector3D("foot" + i + "ImuLinearAcceleration", footIMU.getMeasurementFrame(), registry); - YoFrameVector3D footLinearAccelerationMeasurementInWorld = new YoFrameVector3D("foot" + i + "ImuLinearAccelerationInWorld", worldFrame, registry); - yoFootLinearAccelerationMeasurement.add(footLinearAccelerationMeasurement); - yoFootLinearAccelerationMeasurementInWorld.add(footLinearAccelerationMeasurementInWorld); - - predictedFootTranslations.add(new YoFramePoint3D("predictedFoot" + i + "Translation", worldFrame, registry)); - predictedFootLinearVelocities.add(new YoFrameVector3D("predictedFoot" + i + "LinearVelocity", worldFrame, registry)); - - predictedRelativeFootPositionMeasurements.add(new YoFramePoint3D("predictedRelativeFoot" + i + "PositionMeasurement", baseIMU.getMeasurementFrame(), registry)); - predictedRelativeFootVelocityMeasurements.add(new YoFrameVector3D("predictedRelativeFoot" + i + "VelocityMeasurement", baseIMU.getMeasurementFrame(), registry)); - predictedWorldFootVelocityMeasurements.add(new YoFrameVector3D("predictedWorldFoot" + i + "VelocityMeasurement", worldFrame, registry)); + this.measuredVariables = measuredVariables; + this.currentState = currentState; + this.predictedState = predictedState; + this.cancelGravityFromAccelerationMeasurement = cancelGravityFromAccelerationMeasurement; + this.gravityVector = gravityVector; + this.estimatorDt = estimatorDt; } - parentRegistry.addChild(registry); + public void update() + { + unbiasedAcceleration.sub(measuredVariables.linearAcceleration, currentState.accelBias); + unbiasedAngularVelocity.sub(measuredVariables.angularVelocity, currentState.gyroBias); + // transform the acceleration to world + 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, unbiasedAngularVelocity); + OdometryIndexHelper.toQuaternionFromRotationVector(integratedVelocity, integratedRotation); + predictedState.orientation.multiply(currentState.orientation, integratedRotation); + + // Propagate the bias + predictedState.accelBias.set(currentState.accelBias); + predictedState.gyroBias.set(currentState.gyroBias); + } + } + + private static class SensorMeasurement + { + private final EstimatedVariables baseObserve; + private final EstimatedVariables footObserve; + private final IMUSensorReadOnly baseIMU; + private final IMUSensorReadOnly footIMU; + + // State variables + public final YoVector3D footPredictedRelativePosition; + public final YoVector3D footPredictedRelativeOrientation; + + // Temp variables + private final FramePose3D footPose = new FramePose3D(); + private final Quaternion footOrientationError = new Quaternion(); + private final Vector3D velocityError = new Vector3D(); + + public SensorMeasurement(String prefix, EstimatedVariables baseObserve, EstimatedVariables footObserve, IMUSensorReadOnly baseIMU, + IMUSensorReadOnly footIMU, YoRegistry registry) + { + this.baseObserve = baseObserve; + this.footObserve = footObserve; + this.baseIMU = baseIMU; + this.footIMU = footIMU; + + footPredictedRelativePosition = new YoVector3D(prefix + "PredictedRelativePosition", registry); + footPredictedRelativeOrientation = new YoVector3D(prefix + "PredictedRelativeOrientation", registry); + } + + public void update() + { + // Update the predicted foot position in world + footPredictedRelativePosition.sub(footObserve.translation, baseObserve.translation); + baseObserve.orientation.transform(footPredictedRelativePosition); + + // get the foot pose relative to the base link, which should be entirely based on kinematics + footPose.setToZero(footIMU.getMeasurementFrame()); + footPose.changeFrame(baseIMU.getMeasurementFrame()); + + // FIXME compute the predicted relative orientation + footOrientationError.multiply(footPose.getOrientation().inverse(), baseObserve.orientation.inverse()); + footOrientationError.multiply(footObserve.orientation); + OdometryIndexHelper.logMap(footOrientationError, footPredictedRelativeOrientation); + + + // Compute + // FIXME finish the sensor measurement process + + } + + public void get(int start, DMatrixRMaj measurementToPack) + { + footPredictedRelativePosition.get(start, measurementToPack); + footPredictedRelativeOrientation.get(start + 3, measurementToPack); + } + } + + private static class MeasuredVariables + { + // State providers + private final IMUSensorReadOnly imu; + private final IMUBiasProvider imuBiasProvider; + + // State recorders + public final YoFrameVector3D gyroMeasurementInWorld; + public final YoFrameVector3D gyroMeasurement; + public final YoFrameVector3D linearAccelerationMeasurementInWorld; + public final YoFrameVector3D linearAccelerationMeasurement; + + // Temp variables + private final FrameVector3D linearAcceleration = new FrameVector3D(); + private final FrameVector3D angularVelocity = new FrameVector3D(); + + public MeasuredVariables(String prefix, + IMUSensorReadOnly imu, + IMUBiasProvider imuBiasProvider, + YoRegistry registry) + { + this.imu = imu; + this.imuBiasProvider = imuBiasProvider; + + gyroMeasurementInWorld = new YoFrameVector3D(prefix + "GyroMeasurementInWorld", worldFrame, registry); + gyroMeasurement = new YoFrameVector3D(prefix + "GyroMeasurement", imu.getMeasurementFrame(), registry); + linearAccelerationMeasurementInWorld = new YoFrameVector3D(prefix + "LinearAccelerationMeasurementInWorld", worldFrame, registry); + linearAccelerationMeasurement = new YoFrameVector3D(prefix + "LinearAccelerationMeasurement", imu.getMeasurementFrame(), 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); + + linearAccelerationMeasurementInWorld.setMatchingFrame(linearAcceleration); + linearAccelerationMeasurement.setMatchingFrame(linearAcceleration); + } + + } + private class EstimatedVariables + { + public final YoFramePoint3D translation; + public final YoFrameVector3D linearVelocity; + public final YoFrameQuaternion orientation; + public final YoFrameVector3D accelBias; + public final YoFrameVector3D gyroBias; + + public EstimatedVariables(String prefix) + { + 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", worldFrame, registry); + gyroBias = new YoFrameVector3D(prefix + "GyroBias", worldFrame, registry); + } + + public void set(int start, DMatrixRMaj state) + { + translation.set(start + OdometryIndexHelper.getStatePositionIndex(), state); + linearVelocity.set(start + OdometryIndexHelper.getStateVelocityIndex(), state); + orientation.set(start + OdometryIndexHelper.getStateOrientationIndex(), state); + accelBias.set(start + OdometryIndexHelper.getStateAccelerationBiasIndex(), state); + gyroBias.set(start + OdometryIndexHelper.getStateGyroBiasIndex(), state); + } + + public void get(int start, DMatrixRMaj stateToPack) + { + translation.get(start + OdometryIndexHelper.getStatePositionIndex(), stateToPack); + linearVelocity.get(start + OdometryIndexHelper.getStateVelocityIndex(), stateToPack); + orientation.get(start + OdometryIndexHelper.getStateOrientationIndex(), stateToPack); + accelBias.get(start + OdometryIndexHelper.getStateAccelerationBiasIndex(), stateToPack); + gyroBias.get(start + OdometryIndexHelper.getStateGyroBiasIndex(), stateToPack); + } } public void compute() @@ -206,43 +404,9 @@ public void updateMeasures() // if (!isEstimationEnabled()) // return; - FrameVector3DReadOnly biasInput = imuBiasProvider.getLinearAccelerationBiasInIMUFrame(baseIMU); - Vector3DReadOnly rawInput = baseIMU.getLinearAccelerationMeasurement(); - - linearAcceleration.setReferenceFrame(baseIMU.getMeasurementFrame()); - linearAcceleration.sub(rawInput, biasInput); - - // Update acceleration in world (minus gravity) - if (cancelGravityFromAccelerationMeasurement.getValue()) - { - // FIXME use the estimated orientation of the base instead of the measured one. - linearAcceleration.changeFrame(worldFrame); - linearAcceleration.add(gravityVector); - } - - yoBaseLinearAccelerationMeasurementInWorld.setMatchingFrame(linearAcceleration); - yoBaseLinearAccelerationMeasurement.setMatchingFrame(linearAcceleration); - - for (int i = 0; i < feetIMUs.size(); i++) - { - IMUSensorReadOnly footIMU = feetIMUs.get(i); - FrameVector3DReadOnly footBiasInput = imuBiasProvider.getLinearAccelerationBiasInIMUFrame(footIMU); - Vector3DReadOnly footRawInput = footIMU.getLinearAccelerationMeasurement(); - - linearAcceleration.setReferenceFrame(footIMU.getMeasurementFrame()); - linearAcceleration.sub(footRawInput, footBiasInput); - - // Update acceleration in world (minus gravity) - if (cancelGravityFromAccelerationMeasurement.getValue()) - { - // FIXME use the estimated orientation of the base instead of the measured one. - linearAcceleration.changeFrame(worldFrame); - linearAcceleration.add(gravityVector); - } - - yoFootLinearAccelerationMeasurementInWorld.get(i).setMatchingFrame(linearAcceleration); - yoFootLinearAccelerationMeasurement.get(i).setMatchingFrame(linearAcceleration); - } + baseMeasurement.update(); + for (int i = 0; i < feetMeasurements.size(); i++) + feetMeasurements.get(i).update(); } private final Vector3D angularVelocityMeasurement = new Vector3D(); @@ -319,64 +483,22 @@ private void updateRootJointTwistLinearPart(FrameVector3DReadOnly imuLinearVeloc @Override public DMatrixRMaj processModel(DMatrixRMaj state) { - predictedState.set(state); - estimatedRootJointRotation.set(6, state); + // update the yo variables representing the state + baseCurrentState.set(OdometryIndexHelper.getBasePositionIndex(), state); + for (int i = 0; i < feetIMUs.size(); i++) + { + feetCurrentState.get(i).set(OdometryIndexHelper.getFootPositionIndex(i), state); + } + + // do the prediction + for (int i = 0; i < sensorProcess.size(); i++) + sensorProcess.get(i).update(); - // Compute the predicted position state - // p_k+1 = p_k + v_k * dt - MatrixTools.setMatrixBlock(predictedBaseIMUPositionState, 0, 0, state, 0, 0, 3, 1, 1.0); - MatrixTools.addMatrixBlock(predictedBaseIMUPositionState, 0, 0, state, 3, 0, 3, 1, estimatorDT ); - predictedBaseTranslation.set(predictedBaseIMUPositionState); - - // Compute the predicted velocity state - // v_k+1 = v_k + a_k * dt - // get the acceleration of the base IMU in the base frame, but using the estimated orientation that is internal to the filter. - linearAcceleration.setIncludingFrame(yoBaseLinearAccelerationMeasurement); - estimatedRootJointRotation.transform(linearAcceleration); // TODO make sure this isn't an inverse transform - // figure out the change in velocity based on integrating this acceleration - linearAcceleration.get(predictedBaseIMUVelocityState); - CommonOps_DDRM.scale(estimatorDT, predictedBaseIMUVelocityState); - // add to the velocity the current velocity estimate - MatrixTools.addMatrixBlock(predictedBaseIMUVelocityState, 0, 0, state, 3, 0, 3, 1, 1.0); - predictedBaseLinearVelocity.set(predictedBaseIMUVelocityState); - - // Compute the predicted orientation state by integrating the angular velocity measurement - deltaVector.setAndScale(estimatorDT, baseIMU.getAngularVelocityMeasurement()); - estimatedRootJointRotation.transform(deltaVector); - predictedBaseOrientation.set(estimatedRootJointRotation); - predictedBaseOrientation.append(estimatedRootJointRotation); - - // Pack everything into the predicted state vector - predictedBaseTranslation.get(predictedState); - predictedBaseLinearVelocity.get(3, predictedState); - predictedBaseOrientation.get(6, predictedState); - - // Compute the predicted foot position and velocity states + // update the predicted state from the yo variablized state + basePredictedState.get(OdometryIndexHelper.getBasePositionIndex(), state); for (int i = 0; i < feetIMUs.size(); i++) { - int offset = 10 + i * 6; - // Compute the predicted foot position state - // p_k+1 = p_k + v_k * dt - MatrixTools.setMatrixBlock(predictedFootPositionState, 0, 0, state, offset, 0, 3 , 1, 1.0); - MatrixTools.addMatrixBlock(predictedFootPositionState, 0, 0, state, offset + 3, 0, 3, 1, estimatorDT); - predictedFootTranslations.get(i).set(predictedFootPositionState); - - // Compute the predicted foot velocity state - // v_k+1 = v_k + a_k * dt - // First, get the acceleration of the foot IMU in the base IMU frame, which we're considering here as the root. - linearAcceleration.setIncludingFrame(yoFootLinearAccelerationMeasurement.get(i)); - linearAcceleration.changeFrame(baseIMU.getMeasurementFrame()); - // Now, get the acceleration in the estimated world frame - estimatedRootJointRotation.transform(linearAcceleration); // TODO make sure this isn't an inverse transform - linearAcceleration.get(predictedFootVelocityState); - CommonOps_DDRM.scale(estimatorDT, predictedFootVelocityState); - // add to the velocity the current velocity estimate - MatrixTools.addMatrixBlock(predictedFootVelocityState, 0, 0, state, offset + 3, 0, 3, 1, 1.0); - predictedFootLinearVelocities.get(i).set(predictedFootVelocityState); - - // Pack into the prediction vector. - predictedFootTranslations.get(i).get(offset, predictedState); - predictedFootLinearVelocities.get(i).get(offset + 3, predictedState); + feetPredictedState.get(i).get(OdometryIndexHelper.getFootPositionIndex(i), state); } // return the predicted state @@ -386,42 +508,21 @@ public DMatrixRMaj processModel(DMatrixRMaj state) @Override public DMatrixRMaj measurementModel(DMatrixRMaj state) { - estimatedRootJointRotation.set(6, state); - + // update the yo variables representing the state + baseObserveState.set(OdometryIndexHelper.getBasePositionIndex(), state); for (int i = 0; i < feetIMUs.size(); i++) { - int measurementOffset = 9 * i; - int footStateOffset = 10 + i * 6; - - // Compute the predicted foot relative position measurement - MatrixTools.setMatrixBlock(offsetVector, 0, 0, state, 0, 0, 3 , 1, 1.0); - MatrixTools.addMatrixBlock(offsetVector, 0, 0, state, footStateOffset, 0, 3 , 1, 1.0); - - predictedRelativeFootPositionMeasurements.get(i).set(offsetVector); - estimatedRootJointRotation.inverseTransform(predictedRelativeFootPositionMeasurements.get(i)); // TODO make sure this isn't a transform - - // Compute the predicted foot relative velocity measurement - MatrixTools.setMatrixBlock(offsetVector, 0, 0, state, 3, 0, 3 , 1, 1.0); - MatrixTools.addMatrixBlock(offsetVector, 0, 0, state, footStateOffset + 3, 0, 3 , 1, 1.0); - - predictedRelativeFootVelocityMeasurements.get(i).set(offsetVector); - estimatedRootJointRotation.inverseTransform(predictedRelativeFootVelocityMeasurements.get(i)); // TODO make sure this isn't a transform - - // If the foot is in contact, set the velocity to zero, otherwise use the previous prediction state - if (isFootInContact(i, state)) - { - predictedWorldFootVelocityMeasurements.get(i).setToZero(); - } - else - { - predictedWorldFootVelocityMeasurements.get(i).set(footStateOffset + 3, state); - } + feetObservationState.get(i).set(OdometryIndexHelper.getFootPositionIndex(i), state); + } - predictedRelativeFootPositionMeasurements.get(i).get(measurementOffset, predictedMeasurement); - predictedRelativeFootVelocityMeasurements.get(i).get(measurementOffset + 3, predictedMeasurement); - predictedWorldFootVelocityMeasurements.get(i).get(measurementOffset + 6, predictedMeasurement); + for (int i = 0; i < sensorMeasurements.size(); i++) + { + sensorMeasurements.get(i).update(); + sensorMeasurements.get(i).get(i * 10, predictedMeasurement); // FIXME fix this offset } + estimatedRootJointRotation.set(6, state); + // return the predicted measurement return predictedMeasurement; } From 1649f3ef7740e1620450d050a249df4494122e67 Mon Sep 17 00:00:00 2001 From: Robert Date: Thu, 28 Aug 2025 18:00:39 -0500 Subject: [PATCH 04/18] started getting the EKF running --- .../ExtendedKalmanFilter.java | 3 +- .../odomEKF/OdometryIndexHelper.java | 12 + .../odomEKF/OdometryKalmanFilter.java | 592 +++++++++++++----- 3 files changed, 441 insertions(+), 166 deletions(-) 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 f6a7d070520c..6b90c58c4b47 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 @@ -338,7 +338,8 @@ private void calculateResidualCovarianceAndInverse() CommonOps_DDRM.addEquals(residualCovariance, measurementCovariance); solver.setA(residualCovariance); - solver.invert(inverseResidualCovariance); + CommonOps_DDRM.invert(residualCovariance, inverseResidualCovariance); +// solver.invert(inverseResidualCovariance); } protected void calculateKalmanGain() 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 index 254661f746a3..939b97161e6f 100644 --- 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 @@ -13,6 +13,11 @@ public static int getStateSizePerLink() return 16; } + public static int getObservationSizePerLink() + { + return 15; + } + public static int getStatePositionIndex() { return 0; @@ -91,6 +96,12 @@ public static int getFootGyroBiasIndex(int footNumber) public static void toQuaternionFromRotationVector(Vector3DReadOnly rotation, QuaternionBasics quaternionToPack) { double magnitude = rotation.norm(); + if (magnitude < 1e-7) + { + quaternionToPack.setToZero(); + return; + } + double s = Math.sin(magnitude * 0.5); double qs = Math.cos(magnitude * 0.5); double qx = s * rotation.getX() / magnitude; @@ -112,6 +123,7 @@ public static void toQuaternionFromRotationVectorSmallAngle(Vector3DReadOnly rot 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()); 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 index 050c4b9c0ab2..bd54ad83d356 100644 --- 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 @@ -21,7 +21,6 @@ import us.ihmc.parameterEstimation.ExtendedKalmanFilter; import us.ihmc.sensorProcessing.stateEstimation.IMUSensorReadOnly; import us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.IMUBiasProvider; -import us.ihmc.yoVariables.euclid.YoQuaternion; import us.ihmc.yoVariables.euclid.YoVector3D; import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D; import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePose3D; @@ -29,8 +28,9 @@ import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; import us.ihmc.yoVariables.providers.BooleanProvider; import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; -import java.awt.*; import java.util.ArrayList; import java.util.List; @@ -51,6 +51,17 @@ public class OdometryKalmanFilter extends ExtendedKalmanFilter private final double estimatorDT; private final FrameVector3DReadOnly gravityVector; + private final YoDouble translationCovariance = new YoDouble("translationCovariance", registry); + private final YoDouble velocityCovariance = new YoDouble("velocityCovariance", registry); + private final YoDouble orientationCovariance = new YoDouble("orientationCovariance", registry); + private final YoDouble biasCovariance = new YoDouble("biasCovariance", registry); + + private final YoDouble measuredTranslationCovariance = new YoDouble("measuredTranslationCovariance", registry); + private final YoDouble measuredOrientationCovariance = new YoDouble("measuredOrientationCovariance", registry); + private final YoDouble velocityErrorCovariance = new YoDouble("velocityErrorCovariance", registry); + private final YoDouble contactVelocityCovariance = new YoDouble("contactVelocityCovariance", registry); + private final YoDouble contactOrientationCovariance = new YoDouble("contactOrientationCovariance", registry); + // Internal variables for state holding private final YoFrameVector3D yoBaseAngularVelocityMeasurement; @@ -60,6 +71,7 @@ public class OdometryKalmanFilter extends ExtendedKalmanFilter // Outputs private final DMatrixRMaj predictedState; private final DMatrixRMaj predictedMeasurement; + private final DMatrixRMaj observationVector; private final DMatrixRMaj AMatrix; private final DMatrixRMaj CMatrix; @@ -67,16 +79,14 @@ public class OdometryKalmanFilter extends ExtendedKalmanFilter private final MeasuredVariables baseMeasurement; private final List feetMeasurements = new ArrayList<>(); - private final EstimatedVariables baseCurrentState; - private final EstimatedVariables basePredictedState; - private final List feetCurrentState = new ArrayList<>(); - private final List feetPredictedState = new ArrayList<>(); + private final StateVariables baseProcessState; + private final StateVariables basePredictedState; + private final List feetProcessState = new ArrayList<>(); + private final List feetPredictedState = new ArrayList<>(); - private final List sensorProcess = new ArrayList<>(); - private final List sensorMeasurements = new ArrayList<>(); - - private final EstimatedVariables baseObserveState; - private final List feetObservationState = new ArrayList<>(); + private final List processModels = new ArrayList<>(); + private final List measurementModels = new ArrayList<>(); + private final List observations = new ArrayList<>(); private final YoFrameQuaternion filteredBaseOrientation; private final YoFramePoint3D filteredBaseTranslation; @@ -88,19 +98,13 @@ public class OdometryKalmanFilter extends ExtendedKalmanFilter private final RigidBodyTransformReadOnly transformToRootJoint; - // Temporary variables - private final FrameVector3D linearAcceleration = new FrameVector3D(); - private final FrameVector3D linearVelocity = new FrameVector3D(); - private final Vector3D deltaVector = new Vector3D(); - private final Quaternion estimatedRootJointRotation = new Quaternion(); - private final DMatrixRMaj rotation3x3 = new DMatrixRMaj(3, 3); - - private final DMatrixRMaj offsetVector = new DMatrixRMaj(3, 1); private final DMatrixRMaj footCovariance = new DMatrixRMaj(3, 3); + private final DMatrixRMaj processCovariance; + private final DMatrixRMaj measurementCovariance; + - private final DMatrixRMaj eye3x3 = CommonOps_DDRM.identity(3); - private final Twist twist = new Twist(); + private static final DMatrixRMaj eye3x3 = CommonOps_DDRM.identity(3); public OdometryKalmanFilter(IMUSensorReadOnly baseIMU, List feetIMUs, @@ -110,38 +114,74 @@ public OdometryKalmanFilter(IMUSensorReadOnly baseIMU, double gravitationalAcceleration, YoRegistry parentRegistry) { - super(10 + feetIMUs.size() * 6, 9 * feetIMUs.size()); + super(OdometryIndexHelper.getStateSizePerLink() * (1 + feetIMUs.size()), OdometryIndexHelper.getObservationSizePerLink() * feetIMUs.size()); + + // INitialize covariance + processCovariance = new DMatrixRMaj(getStateSize(), getStateSize()); + measurementCovariance = new DMatrixRMaj(getMeasurementSize(), getMeasurementSize()); + MatrixTools.setDiagonal(processCovariance, 1e-2); + MatrixTools.setDiagonal(measurementCovariance, 1e-3); + setProcessCovariance(processCovariance); + setMeasurementCovariance(measurementCovariance); + + translationCovariance.set(1e-4); + velocityCovariance.set(1e-3); + orientationCovariance.set(1e-4); + biasCovariance.set(1e-6); + + measuredTranslationCovariance.set(1e-5); + measuredOrientationCovariance.set(1e-5); + velocityErrorCovariance.set(1e-4); + contactVelocityCovariance.set(1e-5); + contactOrientationCovariance.set(1e-6); this.baseIMU = baseIMU; this.feetIMUs = feetIMUs; this.imuBiasProvider = baseImuBiasProvider; - this.cancelGravityFromAccelerationMeasurement = cancelGravityFromAccelerationMeasurement; this.estimatorDT = estimatorDT; predictedState = new DMatrixRMaj(getStateSize(), 1); predictedMeasurement = new DMatrixRMaj(getMeasurementSize(), 1); AMatrix = new DMatrixRMaj(getStateSize(), getStateSize()); CMatrix = new DMatrixRMaj(getMeasurementSize(), getStateSize()); + observationVector = new DMatrixRMaj(OdometryIndexHelper.getObservationSizePerLink() * feetIMUs.size(), 1); gravityVector = EuclidFrameFactories.newLinkedFrameVector3DReadOnly(() -> worldFrame, new Vector3D(0, 0, -Math.abs(gravitationalAcceleration))); baseMeasurement = new MeasuredVariables("base", baseIMU, imuBiasProvider, registry); - baseCurrentState = new EstimatedVariables("baseCurrent"); - baseObserveState = new EstimatedVariables("baseObserve"); - basePredictedState = new EstimatedVariables("basePredicted"); - sensorProcess.add(new SensorProcess(baseMeasurement, baseCurrentState, basePredictedState, cancelGravityFromAccelerationMeasurement, gravityVector, estimatorDT)); + baseProcessState = new StateVariables("baseProcessState"); + basePredictedState = new StateVariables("basePredicted"); + processModels.add(new ProcessModel(baseMeasurement, + baseProcessState, + basePredictedState, + cancelGravityFromAccelerationMeasurement, + gravityVector, + estimatorDT)); for (int i = 0; i < feetIMUs.size(); i++) { MeasuredVariables footMeasurements = new MeasuredVariables("foot" + i, feetIMUs.get(i), imuBiasProvider, registry); - EstimatedVariables footCurrentState = new EstimatedVariables("foot" + i + "Current"); - EstimatedVariables footObservationState = new EstimatedVariables("foot" + i + "Observe"); - EstimatedVariables footPredictedState = new EstimatedVariables("foot" + i + "Predicted"); + StateVariables footProcessState = new StateVariables("foot" + i + "ProcessState"); + StateVariables footPredictedState = new StateVariables("foot" + i + "PredictedState"); feetMeasurements.add(footMeasurements); - feetObservationState.add(footObservationState); - feetCurrentState.add(footCurrentState); + feetProcessState.add(footProcessState); feetPredictedState.add(footPredictedState); - sensorProcess.add(new SensorProcess(footMeasurements, footCurrentState, footPredictedState, cancelGravityFromAccelerationMeasurement, gravityVector, estimatorDT)); - sensorMeasurements.add(new SensorMeasurement("foot" + i, baseObserveState, footObservationState, baseIMU, feetIMUs.get(i), registry)); + + observations.add(new Observation("foot" + i + "Observation", baseIMU, feetIMUs.get(i), footMeasurements, registry)); + + processModels.add(new ProcessModel(footMeasurements, + footProcessState, + footPredictedState, + cancelGravityFromAccelerationMeasurement, + gravityVector, + estimatorDT)); + measurementModels.add(new MeasurementModel("foot" + i, + basePredictedState, + baseMeasurement, + footPredictedState, + baseIMU, + feetIMUs.get(i), + gravityVector, + registry)); } filteredBaseOrientation = new YoFrameQuaternion("filteredBaseOrientation", worldFrame, registry); @@ -155,18 +195,18 @@ public OdometryKalmanFilter(IMUSensorReadOnly baseIMU, transformToRootJoint = baseIMU.getMeasurementFrame().getTransformToDesiredFrame(baseIMU.getMeasurementLink().getParentJoint().getFrameAfterJoint()); yoBaseAngularVelocityMeasurement = new YoFrameVector3D("baseImuLinearAccelerationInWorld", baseIMU.getMeasurementFrame(), registry); - yoRootJointAngularVelocityMeasurement = new YoFrameVector3D("rootJointImuLinearAccelerationInWorld", baseIMU.getMeasurementFrame(), registry); + yoRootJointAngularVelocityMeasurement = new YoFrameVector3D("rootJointImuLinearAcceleration", baseIMU.getMeasurementFrame(), registry); yoRootJointAngularVelocityMeasurementInWorld = new YoFrameVector3D("rootJointImuLinearAccelerationInWorld", worldFrame, registry); parentRegistry.addChild(registry); } - private static class SensorProcess + private static class ProcessModel { // State providers private final MeasuredVariables measuredVariables; - private final EstimatedVariables currentState; - private final EstimatedVariables predictedState; + private final StateVariables currentState; + private final StateVariables predictedState; private final BooleanProvider cancelGravityFromAccelerationMeasurement; private final FrameVector3DReadOnly gravityVector; private final double estimatorDt; @@ -177,15 +217,15 @@ private static class SensorProcess private final Vector3D integratedVelocity = new Vector3D(); private final Quaternion integratedRotation = new Quaternion(); - public SensorProcess(MeasuredVariables measuredVariables, - EstimatedVariables currentState, - EstimatedVariables predictedState, - BooleanProvider cancelGravityFromAccelerationMeasurement, - FrameVector3DReadOnly gravityVector, - double estimatorDt) + public ProcessModel(MeasuredVariables measuredVariables, + StateVariables processState, + StateVariables predictedState, + BooleanProvider cancelGravityFromAccelerationMeasurement, + FrameVector3DReadOnly gravityVector, + double estimatorDt) { this.measuredVariables = measuredVariables; - this.currentState = currentState; + this.currentState = processState; this.predictedState = predictedState; this.cancelGravityFromAccelerationMeasurement = cancelGravityFromAccelerationMeasurement; this.gravityVector = gravityVector; @@ -194,8 +234,8 @@ public SensorProcess(MeasuredVariables measuredVariables, public void update() { - unbiasedAcceleration.sub(measuredVariables.linearAcceleration, currentState.accelBias); - unbiasedAngularVelocity.sub(measuredVariables.angularVelocity, currentState.gyroBias); + unbiasedAcceleration.sub(measuredVariables.linearAccelerationMeasurement, currentState.accelBias); + unbiasedAngularVelocity.sub(measuredVariables.gyroMeasurement, currentState.gyroBias); // transform the acceleration to world currentState.orientation.transform(unbiasedAcceleration); if (cancelGravityFromAccelerationMeasurement.getValue()) @@ -221,59 +261,160 @@ public void update() } } - private static class SensorMeasurement + private static class Observation + { + // State providers + private final IMUSensorReadOnly baseIMU; + private final IMUSensorReadOnly footIMU; + private final MeasuredVariables footMeasurements; + + // State variables + private final YoVector3D footPositionFromKinematics; + + // Temp variables + private final FramePoint3D footPosition = new FramePoint3D(); + + public Observation(String prefix, IMUSensorReadOnly baseIMU, IMUSensorReadOnly footIMU, MeasuredVariables footMeasurements, YoRegistry registry) + { + this.baseIMU = baseIMU; + this.footIMU = footIMU; + this.footMeasurements = footMeasurements; + + footPositionFromKinematics = new YoVector3D(prefix + "PositionFromKinematics", registry); + } + + public void update() + { + footPosition.setToZero(footIMU.getMeasurementFrame()); + footPosition.changeFrame(baseIMU.getMeasurementFrame()); + + footPositionFromKinematics.set(footPosition); + } + + public void get(int startRow, DMatrixRMaj observation) + { + footPositionFromKinematics.get(startRow, observation); + footMeasurements.linearAccelerationMeasurement.get(startRow + 6, observation); + } + } + + private static class MeasurementModel { - private final EstimatedVariables baseObserve; - private final EstimatedVariables footObserve; + private final StateVariables baseState; + private final MeasuredVariables baseMeasurement; + private final StateVariables footState; private final IMUSensorReadOnly baseIMU; private final IMUSensorReadOnly footIMU; + private final FrameVector3DReadOnly gravityVector; // State variables public final YoVector3D footPredictedRelativePosition; public final YoVector3D footPredictedRelativeOrientation; + public final YoVector3D footPredictedLinearVelocityError; + public final YoVector3D footPredictedLinearVelocity; + public final YoVector3D footPredictedGravityVector; + + public final YoBoolean isInContact; // Temp variables private final FramePose3D footPose = new FramePose3D(); private final Quaternion footOrientationError = new Quaternion(); private final Vector3D velocityError = new Vector3D(); + private final Vector3D unbiasedAngularVelocity = new Vector3D(); - public SensorMeasurement(String prefix, EstimatedVariables baseObserve, EstimatedVariables footObserve, IMUSensorReadOnly baseIMU, - IMUSensorReadOnly footIMU, YoRegistry registry) + public MeasurementModel(String prefix, + StateVariables baseState, + MeasuredVariables baseMeasurement, + StateVariables footState, + IMUSensorReadOnly baseIMU, + IMUSensorReadOnly footIMU, + FrameVector3DReadOnly gravityVector, + YoRegistry registry) { - this.baseObserve = baseObserve; - this.footObserve = footObserve; + this.baseState = baseState; + this.baseMeasurement = baseMeasurement; + this.footState = footState; this.baseIMU = baseIMU; this.footIMU = footIMU; + this.gravityVector = gravityVector; footPredictedRelativePosition = new YoVector3D(prefix + "PredictedRelativePosition", registry); footPredictedRelativeOrientation = new YoVector3D(prefix + "PredictedRelativeOrientation", registry); + footPredictedLinearVelocityError = new YoVector3D(prefix + "PredictedLinearVelocityError", registry); + footPredictedLinearVelocity = new YoVector3D(prefix + "PredictedLinearVelocity", registry); + footPredictedGravityVector = new YoVector3D(prefix + "PredictedGravityVector", registry); + isInContact = new YoBoolean(prefix + "IsInContact", registry); } - public void update() + /** + * This is equation 35 + */ + public void update(DMatrixRMaj footCovariance) { - // Update the predicted foot position in world - footPredictedRelativePosition.sub(footObserve.translation, baseObserve.translation); - baseObserve.orientation.transform(footPredictedRelativePosition); + isInContact.set(isFootInContact(footCovariance)); + + // First row. Update the predicted foot position of the foot relative to the base in the base frame + footPredictedRelativePosition.sub(footState.translation, baseState.translation); + baseState.orientation.transform(footPredictedRelativePosition); - // get the foot pose relative to the base link, which should be entirely based on kinematics + // Second row. get the foot pose relative to the base link, which should be entirely based on kinematics footPose.setToZero(footIMU.getMeasurementFrame()); footPose.changeFrame(baseIMU.getMeasurementFrame()); - // FIXME compute the predicted relative orientation - footOrientationError.multiply(footPose.getOrientation().inverse(), baseObserve.orientation.inverse()); - footOrientationError.multiply(footObserve.orientation); + // Update the predicted foot orientation of the foot relative to the base in the base frame + footOrientationError.set(footPose.getOrientation()); + footOrientationError.multiplyConjugateBoth(baseState.orientation); + footOrientationError.multiply(footState.orientation); OdometryIndexHelper.logMap(footOrientationError, footPredictedRelativeOrientation); + // Compute the linear velocity error in the base frame + unbiasedAngularVelocity.sub(baseMeasurement.gyroMeasurement, baseState.gyroBias); + velocityError.sub(baseState.linearVelocity, footState.linearVelocity); + baseState.orientation.inverseTransform(velocityError); - // Compute - // FIXME finish the sensor measurement process + // FIXME this likely changes based on the contact conditions + footPredictedLinearVelocityError.cross(unbiasedAngularVelocity, footPose.getPosition()); + footPredictedLinearVelocityError.scale(-1.0); + footPredictedLinearVelocityError.sub(velocityError); + // FIXME get the estimated foot linear velocity from the kinematics, and add it to here. That is, get the one using the MovingReferenceFrame, relative to the base IMU. + + if (isInContact.getBooleanValue()) + return; + + // Fourth row. Set the predicted linear velocity + footPredictedLinearVelocity.set(footState.linearVelocity); + + // Fifth row. Set the predicted gravity vector + footState.orientation.transform(gravityVector, footPredictedGravityVector); + footPredictedGravityVector.add(footState.accelBias); + } + + private final DMatrixRMaj vector = new DMatrixRMaj(3, 1); + private final DMatrixRMaj tempVector = new DMatrixRMaj(3, 1); + private final DMatrixRMaj tempScalar = new DMatrixRMaj(3, 1); + + private boolean isFootInContact(DMatrixRMaj footCovariance) + { + footState.translation.get(vector); + + CommonOps_DDRM.mult(footCovariance, vector, tempVector); + CommonOps_DDRM.multTransA(vector, tempVector, tempScalar); + + // tests the mahalonobis distance of the foot velocity being below a certain threshold. + return tempScalar.get(0, 0) < 0.2; // FIXME extract magic number } public void get(int start, DMatrixRMaj measurementToPack) { footPredictedRelativePosition.get(start, measurementToPack); footPredictedRelativeOrientation.get(start + 3, measurementToPack); + footPredictedLinearVelocityError.get(start + 6, measurementToPack); + if (isInContact.getBooleanValue()) + return; + + footPredictedLinearVelocity.get(start + 9, measurementToPack); + footPredictedGravityVector.get(start + 12, measurementToPack); } } @@ -293,10 +434,7 @@ private static class MeasuredVariables private final FrameVector3D linearAcceleration = new FrameVector3D(); private final FrameVector3D angularVelocity = new FrameVector3D(); - public MeasuredVariables(String prefix, - IMUSensorReadOnly imu, - IMUBiasProvider imuBiasProvider, - YoRegistry registry) + public MeasuredVariables(String prefix, IMUSensorReadOnly imu, IMUBiasProvider imuBiasProvider, YoRegistry registry) { this.imu = imu; this.imuBiasProvider = imuBiasProvider; @@ -305,7 +443,6 @@ public MeasuredVariables(String prefix, gyroMeasurement = new YoFrameVector3D(prefix + "GyroMeasurement", imu.getMeasurementFrame(), registry); linearAccelerationMeasurementInWorld = new YoFrameVector3D(prefix + "LinearAccelerationMeasurementInWorld", worldFrame, registry); linearAccelerationMeasurement = new YoFrameVector3D(prefix + "LinearAccelerationMeasurement", imu.getMeasurementFrame(), registry); - } public void update() @@ -330,9 +467,9 @@ public void update() linearAccelerationMeasurementInWorld.setMatchingFrame(linearAcceleration); linearAccelerationMeasurement.setMatchingFrame(linearAcceleration); } - } - private class EstimatedVariables + + private class StateVariables { public final YoFramePoint3D translation; public final YoFrameVector3D linearVelocity; @@ -340,7 +477,7 @@ private class EstimatedVariables public final YoFrameVector3D accelBias; public final YoFrameVector3D gyroBias; - public EstimatedVariables(String prefix) + public StateVariables(String prefix) { translation = new YoFramePoint3D(prefix + "Translation", worldFrame, registry); linearVelocity = new YoFrameVector3D(prefix + "LinearVelocity", worldFrame, registry); @@ -370,15 +507,20 @@ public void get(int start, DMatrixRMaj stateToPack) public void compute() { - updateMeasures(); - updateRootJointTwistAngularPart(); + updateCovariance(); + + baseMeasurement.update(); + for (int i = 0; i < feetMeasurements.size(); i++) + feetMeasurements.get(i).update(); - // FIXME populate the observations, which are - // FIXME 1. the relative distance between the IMU and the foot in the IMU frame - // FIXME 2. the relative linear velocity between the IMU and the foot in the IMU frame - // FIXME 3. the absolute linear velocity of the foot + for (int i = 0; i < observations.size(); i++) + { + observations.get(i).update(); + observations.get(i).get(i * OdometryIndexHelper.getObservationSizePerLink(), observationVector); + } + updateRootJointTwistAngularPart(); // TODO maybe do this after we update everything based on the estimate? - DMatrixRMaj stateEstimate = calculateEstimate(); + DMatrixRMaj stateEstimate = calculateEstimate(observationVector); filteredBaseTranslation.set(stateEstimate); filteredBaseLinearVelocity.set(3, stateEstimate); @@ -390,25 +532,58 @@ public void compute() estimatedRootJointPose.set(tempPose); estimatedRootJointAngularVelocity.set(yoRootJointAngularVelocityMeasurementInWorld); - updateRootJointTwistLinearPart(filteredBaseLinearVelocity, - estimatedRootJointAngularVelocity, - estimatedRootJointLinearVelocity); + updateRootJointTwistLinearPart(filteredBaseLinearVelocity, estimatedRootJointAngularVelocity, estimatedRootJointLinearVelocity); } - private final FramePose3D tempPose = new FramePose3D(); - - - public void updateMeasures() + private void updateCovariance() { - // TODO re-enable this -// if (!isEstimationEnabled()) -// return; + for (int i = 0; i < feetIMUs.size() + 1; i++) + { + int index = i * OdometryIndexHelper.getStateSizePerLink(); + processCovariance.set(index, index++, translationCovariance.getValue()); + processCovariance.set(index, index++, translationCovariance.getValue()); + processCovariance.set(index, index++, translationCovariance.getValue()); + processCovariance.set(index, index++, velocityCovariance.getValue()); + processCovariance.set(index, index++, velocityCovariance.getValue()); + processCovariance.set(index, index++, velocityCovariance.getValue()); + processCovariance.set(index, index++, orientationCovariance.getValue()); + processCovariance.set(index, index++, orientationCovariance.getValue()); + processCovariance.set(index, index++, orientationCovariance.getValue()); + processCovariance.set(index, index++, orientationCovariance.getValue()); + processCovariance.set(index, index++, biasCovariance.getValue()); + processCovariance.set(index, index++, biasCovariance.getValue()); + processCovariance.set(index, index++, biasCovariance.getValue()); + processCovariance.set(index, index++, biasCovariance.getValue()); + processCovariance.set(index, index++, biasCovariance.getValue()); + processCovariance.set(index, index, biasCovariance.getValue()); + } - baseMeasurement.update(); - for (int i = 0; i < feetMeasurements.size(); i++) - feetMeasurements.get(i).update(); + for (int i = 0; i < feetIMUs.size(); i++) + { + int index = i * OdometryIndexHelper.getObservationSizePerLink(); + measurementCovariance.set(index, index++, measuredTranslationCovariance.getValue()); + measurementCovariance.set(index, index++, measuredTranslationCovariance.getValue()); + measurementCovariance.set(index, index++, measuredTranslationCovariance.getValue()); + measurementCovariance.set(index, index++, measuredOrientationCovariance.getValue()); + measurementCovariance.set(index, index++, measuredOrientationCovariance.getValue()); + measurementCovariance.set(index, index++, measuredOrientationCovariance.getValue()); + measurementCovariance.set(index, index++, velocityErrorCovariance.getValue()); + measurementCovariance.set(index, index++, velocityErrorCovariance.getValue()); + measurementCovariance.set(index, index++, velocityErrorCovariance.getValue()); + measurementCovariance.set(index, index++, contactVelocityCovariance.getValue()); + measurementCovariance.set(index, index++, contactVelocityCovariance.getValue()); + measurementCovariance.set(index, index++, contactVelocityCovariance.getValue()); + measurementCovariance.set(index, index++, contactOrientationCovariance.getValue()); + measurementCovariance.set(index, index++, contactOrientationCovariance.getValue()); + measurementCovariance.set(index, index, contactOrientationCovariance.getValue()); + } + + 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. */ @@ -483,16 +658,18 @@ private void updateRootJointTwistLinearPart(FrameVector3DReadOnly imuLinearVeloc @Override public DMatrixRMaj processModel(DMatrixRMaj state) { + predictedState.zero(); + // update the yo variables representing the state - baseCurrentState.set(OdometryIndexHelper.getBasePositionIndex(), state); + baseProcessState.set(OdometryIndexHelper.getBasePositionIndex(), state); for (int i = 0; i < feetIMUs.size(); i++) { - feetCurrentState.get(i).set(OdometryIndexHelper.getFootPositionIndex(i), state); + feetProcessState.get(i).set(OdometryIndexHelper.getFootPositionIndex(i), state); } // do the prediction - for (int i = 0; i < sensorProcess.size(); i++) - sensorProcess.get(i).update(); + for (int i = 0; i < processModels.size(); i++) + processModels.get(i).update(); // update the predicted state from the yo variablized state basePredictedState.get(OdometryIndexHelper.getBasePositionIndex(), state); @@ -506,65 +683,38 @@ public DMatrixRMaj processModel(DMatrixRMaj state) } @Override - public DMatrixRMaj measurementModel(DMatrixRMaj state) + public DMatrixRMaj measurementModel(DMatrixRMaj predictedState) { - // update the yo variables representing the state - baseObserveState.set(OdometryIndexHelper.getBasePositionIndex(), state); + 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++) { - feetObservationState.get(i).set(OdometryIndexHelper.getFootPositionIndex(i), state); - } + int row = OdometryIndexHelper.getFootVelocityIndex(i); + MatrixTools.setMatrixBlock(footCovariance, 0, 0, getCovariance(), row, row, 3, 3, 1.0); - for (int i = 0; i < sensorMeasurements.size(); i++) - { - sensorMeasurements.get(i).update(); - sensorMeasurements.get(i).get(i * 10, predictedMeasurement); // FIXME fix this offset + measurementModels.get(i).update(footCovariance); + measurementModels.get(i).get(i * OdometryIndexHelper.getObservationSizePerLink(), predictedMeasurement); } - estimatedRootJointRotation.set(6, state); - // return the predicted measurement return predictedMeasurement; } - private final DMatrixRMaj temp = new DMatrixRMaj(3, 1); - private final DMatrixRMaj tempScalar = new DMatrixRMaj(1, 1); - private boolean isFootInContact(int footIndex, DMatrixRMaj state) - { - int startRow = 10 + footIndex * 6; - CommonOps_DDRM.extract(state, startRow, startRow + 3, 0, 1, offsetVector, 0, 0); - CommonOps_DDRM.extract(getCovariance(), startRow, startRow + 3, startRow, startRow + 3, footCovariance, 0, 0); - - CommonOps_DDRM.mult(footCovariance, offsetVector, temp); - CommonOps_DDRM.multTransA(offsetVector, temp, tempScalar); - - // tests the mahalonobis distance of the foot velocity being below a certain threshold. - return tempScalar.get(0, 0) < 0.2; - } - @Override protected DMatrixRMaj linearizeProcessModel(DMatrixRMaj previousState) { - // Populate the gradient of the base position w.r.t. the state - CommonOps_DDRM.insert(eye3x3, AMatrix, 0, 0); - MatrixTools.setMatrixBlock(AMatrix, 0, 3, eye3x3, 0, 0, 3, 3, estimatorDT); - - // Populate the gradient of the base velocity w.r.t. the state - CommonOps_DDRM.insert(eye3x3, AMatrix, 3, 3); - // FIXME add the effect of the gradient of the base rotation - - // FIXME do the gradient crap for the base orientation, which is going to be frustrating + // make sure to call zero, as the compute process model stuff has a bunch of add operators. + AMatrix.zero(); + int rowOffset = 0; + int colOffset = 0; + computeProcessJacobian(baseMeasurement, baseProcessState, estimatorDT, rowOffset, colOffset, AMatrix); for (int i = 0; i < feetIMUs.size(); i++) { - int stateOffset = 10 + i * 6; - // Populate the gradient of the foot position w.r.t. the state - CommonOps_DDRM.insert(eye3x3, AMatrix, stateOffset, stateOffset); - MatrixTools.setMatrixBlock(AMatrix, stateOffset, stateOffset + 3, eye3x3, 0, 0, 3, 3, estimatorDT); - - // Populate the gradient of the foot position w.r.t. the state - // FIXME add the effect of the gradient of the base rotation - CommonOps_DDRM.insert(eye3x3, AMatrix, stateOffset + 3, stateOffset + 3); + rowOffset += OdometryIndexHelper.getStateSizePerLink(); + colOffset += OdometryIndexHelper.getStateSizePerLink(); + computeProcessJacobian(feetMeasurements.get(i), feetProcessState.get(i), estimatorDT, rowOffset, colOffset, AMatrix); } return AMatrix; @@ -573,34 +723,17 @@ protected DMatrixRMaj linearizeProcessModel(DMatrixRMaj previousState) @Override protected DMatrixRMaj linearizeMeasurementModel(DMatrixRMaj predictedState) { - estimatedRootJointRotation.set(6, predictedState); - toRotationMatrix(estimatedRootJointRotation, rotation3x3); - - CommonOps_DDRM.transpose(rotation3x3); - + // make sure to call zero, as the compute process model stuff has a bunch of add operators. + CMatrix.zero(); + int rowOffset = 0; + int colOffset = 0; for (int i = 0; i < feetIMUs.size(); i++) { - int footStateOffset = 10 + i * 6; - int measureOffset = 9 * i; - // R^T * (s_k - p_k) - CommonOps_DDRM.insert(rotation3x3, CMatrix, measureOffset, footStateOffset); - MatrixTools.setMatrixBlock(CMatrix, measureOffset, 0, rotation3x3, 0, 0, 3, 3, -1.0); - - MatrixTools.setMatrixBlock(offsetVector, 0, 0, predictedState, footStateOffset, 0, 3, 1, 1.0); - MatrixTools.addMatrixBlock(offsetVector, 0, 0, predictedState, 0, 0, 3, 1, -1.0); - - // TODO add the effect of the gradient of the base rotation - - // R^T * (v_k - sDot_k) - CommonOps_DDRM.insert(rotation3x3, CMatrix, measureOffset + 3, 3); - MatrixTools.setMatrixBlock(CMatrix, measureOffset + 3, footStateOffset + 3, rotation3x3, 0, 0, 3, 3, -1.0); - // TODO add the effect of the gradient of the base rotation - - MatrixTools.setMatrixBlock(offsetVector, 0, 0, predictedState, 3, 0, 3, 1, 1.0); - MatrixTools.addMatrixBlock(offsetVector, 0, 0, predictedState, footStateOffset + 3, 0, 3, 1, -1.0); - - // sDot_k - CommonOps_DDRM.insert(eye3x3, CMatrix, measureOffset + 6, footStateOffset + 3); + boolean footIsInContact = measurementModels.get(i).isInContact.getBooleanValue(); + computeBaseMeasurementJacobian(basePredictedState, feetPredictedState.get(i), rowOffset, CMatrix); + computeFootMeasurementJacobian(footIsInContact, basePredictedState, feetPredictedState.get(i), gravityVector, rowOffset, colOffset + 15, CMatrix); + rowOffset += OdometryIndexHelper.getObservationSizePerLink(); + colOffset += OdometryIndexHelper.getStateSizePerLink(); } return CMatrix; @@ -671,4 +804,133 @@ static void toRotationMatrixInverse(double qx, double qy, double qz, double qs, rotationMatrix.set(2, 1, 2.0 * (yz - xw)); rotationMatrix.set(1, 2, 2.0 * (yz + xw)); } + + /** + * This is equation 39 + */ + static void computeProcessJacobian(MeasuredVariables measuredVariables, StateVariables stateVariables, double estimateDT, int rowOffset, int colOffset, DMatrixRMaj jacobianToPack) + { + // Do the partial derivative with respect to the base position state + CommonOps_DDRM.insert(eye3x3, jacobianToPack, rowOffset + OdometryIndexHelper.getStatePositionIndex(), colOffset + OdometryIndexHelper.getStatePositionIndex()); + MatrixTools.setMatrixBlock(jacobianToPack, + rowOffset + OdometryIndexHelper.getStatePositionIndex(), + colOffset + OdometryIndexHelper.getStateVelocityIndex(), + eye3x3, + 0, + 0, + 3, + 3, + estimateDT); + + // Do the partial derivative with respect to the base velocity state FIXME lots of garbage + CommonOps_DDRM.insert(eye3x3, jacobianToPack, rowOffset + OdometryIndexHelper.getStateVelocityIndex(), colOffset + OdometryIndexHelper.getStateVelocityIndex()); + + DMatrixRMaj rotationMatrix = new DMatrixRMaj(3, 3); + DMatrixRMaj skewMatrix = new DMatrixRMaj(3, 3); + Vector3D unbiasedAcceleration = new Vector3D(); + unbiasedAcceleration.sub(measuredVariables.linearAccelerationMeasurement, stateVariables.accelBias); + toRotationMatrix(stateVariables.orientation, rotationMatrix); + toSkewSymmetricMatrix(unbiasedAcceleration, skewMatrix); + MatrixTools.multAddBlock(-estimateDT, rotationMatrix, skewMatrix, jacobianToPack, rowOffset + OdometryIndexHelper.getStateVelocityIndex(), colOffset + OdometryIndexHelper.getStateOrientationIndex()); + + MatrixTools.setMatrixBlock(jacobianToPack, rowOffset + OdometryIndexHelper.getStateVelocityIndex(), colOffset + OdometryIndexHelper.getStateGyroBiasIndex(), rotationMatrix, 0, 0, 3, 3, -estimateDT); + + // Do the partial derivative with respect to the orientation state + Vector3D unbiasedVelocity = new Vector3D(); + unbiasedVelocity.sub(measuredVariables.gyroMeasurement, stateVariables.gyroBias); + toSkewSymmetricMatrix(unbiasedVelocity, skewMatrix); + MatrixTools.setMatrixBlock(jacobianToPack, rowOffset + OdometryIndexHelper.getStateOrientationIndex(), colOffset + OdometryIndexHelper.getStateOrientationIndex(), rotationMatrix, 0, 0, 3, 3, -estimateDT); + MatrixTools.addMatrixBlock(jacobianToPack, rowOffset + OdometryIndexHelper.getStateOrientationIndex(), colOffset + OdometryIndexHelper.getStateOrientationIndex(), eye3x3, 0, 0, 3, 3, 1.0); + MatrixTools.setMatrixBlock(jacobianToPack, rowOffset + OdometryIndexHelper.getStateOrientationIndex(), colOffset + OdometryIndexHelper.getStateAccelerationBiasIndex(), eye3x3, 0, 0, 3, 3, -estimateDT); + + // Do the partial derivative with respect to the acceleration bias state + MatrixTools.setMatrixBlock(jacobianToPack, rowOffset + OdometryIndexHelper.getStateAccelerationBiasIndex(), colOffset + OdometryIndexHelper.getStateAccelerationBiasIndex(), eye3x3, 0, 0, 3, 3, 1.0); + // Do the partial derivative with respect to the gyro bias state + MatrixTools.setMatrixBlock(jacobianToPack, rowOffset + OdometryIndexHelper.getStateGyroBiasIndex(), colOffset + OdometryIndexHelper.getStateGyroBiasIndex(), eye3x3, 0, 0, 3, 3, 1.0); + } + + /** + * This is equation 41 + */ + private void computeBaseMeasurementJacobian(StateVariables baseState, StateVariables footState, int rowOffset, DMatrixRMaj jacobianToPack) + { + DMatrixRMaj rotationMatrix = new DMatrixRMaj(3, 3); + toRotationMatrix(baseState.orientation, rotationMatrix); + CommonOps_DDRM.transpose(rotationMatrix); + + Vector3D footPositionError = new Vector3D(); + Vector3D footVelocityError = new Vector3D(); + footPositionError.sub(footState.translation, baseState.translation); + footVelocityError.sub(footState.linearVelocity, baseState.linearVelocity); + baseState.orientation.inverseTransform(footPositionError); + baseState.orientation.inverseTransform(footVelocityError); + + DMatrixRMaj skewMatrix = new DMatrixRMaj(3, 3); + toSkewSymmetricMatrix(footPositionError, skewMatrix); + + // First row + MatrixTools.setMatrixBlock(jacobianToPack, rowOffset, 0, rotationMatrix, 0, 0, 3, 3, -1.0); + CommonOps_DDRM.insert(skewMatrix, jacobianToPack, rowOffset, 6); + + // FIXME do the second row + + // Third row + toSkewSymmetricMatrix(footVelocityError, skewMatrix); + + MatrixTools.setMatrixBlock(jacobianToPack, rowOffset + 6, 3, rotationMatrix, 0, 0, 3, 3, -1.0); + CommonOps_DDRM.insert(skewMatrix, jacobianToPack, rowOffset+ 6, 6); + // FIXME do the last term, in the fifth column + } + + /** + * This is equation 42 + */ + private void computeFootMeasurementJacobian(boolean isInContact, StateVariables baseState, StateVariables footState, FrameVector3DReadOnly gravityVector, int rowOffset, int colOffset, DMatrixRMaj jacobianToPack) + { + DMatrixRMaj rotationMatrix = new DMatrixRMaj(3, 3); + toRotationMatrix(baseState.orientation, rotationMatrix); + CommonOps_DDRM.transpose(rotationMatrix); + + Vector3D footPositionError = new Vector3D(); + Vector3D footVelocityError = new Vector3D(); + footPositionError.sub(footState.translation, baseState.translation); + footVelocityError.sub(footState.linearVelocity, baseState.linearVelocity); + baseState.orientation.inverseTransform(footPositionError); + baseState.orientation.inverseTransform(footVelocityError); + + DMatrixRMaj skewMatrix = new DMatrixRMaj(3, 3); + toSkewSymmetricMatrix(footPositionError, skewMatrix); + + // First row + CommonOps_DDRM.insert(rotationMatrix, jacobianToPack, rowOffset, colOffset); + + // FIXME do the second row + + // Third row + CommonOps_DDRM.insert(rotationMatrix, jacobianToPack, rowOffset + 6, colOffset + 3); + + if (!isInContact) + return; + + // Fourth row + CommonOps_DDRM.insert(eye3x3, jacobianToPack, rowOffset + 9, colOffset + 3); + + // Fifth row + Vector3D gravity = new Vector3D(gravityVector); + footState.orientation.inverseTransform(gravity); + toSkewSymmetricMatrix(gravityVector, skewMatrix); + CommonOps_DDRM.insert(skewMatrix, jacobianToPack, rowOffset + 12, colOffset + 6); + CommonOps_DDRM.insert(eye3x3, jacobianToPack, rowOffset + 12, colOffset + 9); + } + + static void toSkewSymmetricMatrix(Vector3DReadOnly vector, DMatrixRMaj matrixToPack) + { + matrixToPack.zero(); + matrixToPack.set(0, 1, -vector.getZ()); + matrixToPack.set(0, 2, vector.getY()); + matrixToPack.set(1, 0, vector.getZ()); + matrixToPack.set(1, 2, -vector.getX()); + matrixToPack.set(2, 0, -vector.getY()); + matrixToPack.set(2, 1, -vector.getX()); + } } From 77da4335fa57e9ab8d2c445ebea74de351d20cfe Mon Sep 17 00:00:00 2001 From: Robert Date: Fri, 29 Aug 2025 16:54:47 -0500 Subject: [PATCH 05/18] started breaking up the EKF, and getting things to run --- .../ExtendedKalmanFilter.java | 5 + .../ihmc/robotics/sensors/IMUDefinition.java | 9 +- .../ihmc/sensorProcessing/imu/IMUSensor.java | 5 +- .../stateEstimation/IMUSensorReadOnly.java | 3 +- .../DRCKinematicsBasedStateEstimator.java | 6 +- .../odomEKF/MeasurementModel.java | 244 +++++++ .../odomEKF/ObservationModel.java | 21 + .../odomEKF/OdometryKalmanFilter.java | 618 ++++-------------- .../odomEKF/OdometryTools.java | 90 +++ .../odomEKF/ProcessModel.java | 123 ++++ .../odomEKF/OdometryKalmanFilterTest.java | 57 -- .../odomEKF/OdometryToolsTest.java | 124 ++++ 12 files changed, 764 insertions(+), 541 deletions(-) create mode 100644 ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/MeasurementModel.java create mode 100644 ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/ObservationModel.java create mode 100644 ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryTools.java create mode 100644 ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/ProcessModel.java delete mode 100644 ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryKalmanFilterTest.java create mode 100644 ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryToolsTest.java 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 6b90c58c4b47..a330c7c34f87 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 @@ -210,6 +210,11 @@ public ExtendedKalmanFilter(DMatrixRMaj initialState, DMatrixRMaj initialCovaria covariance.set(initialCovariance); } + public void initializeState(DMatrixRMaj state) + { + this.state.set(state); + } + public int getStateSize() { return stateSize; 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 68792aaba966..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 @@ -344,8 +344,7 @@ public void doControl() for (int i = 0; i < footSwitchList.size(); i++) footSwitchList.get(i).update(); - // TODO should this also consume some of the foot switch data? - odometry.compute(); + switch (operatingMode.getEnumValue()) { @@ -358,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/odomEKF/MeasurementModel.java b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/MeasurementModel.java new file mode 100644 index 000000000000..6aca27f48f58 --- /dev/null +++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/MeasurementModel.java @@ -0,0 +1,244 @@ +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.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF.OdometryKalmanFilter.MeasuredVariables; +import us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF.OdometryKalmanFilter.StateVariables; +import us.ihmc.yoVariables.euclid.YoQuaternion; +import us.ihmc.yoVariables.euclid.YoVector3D; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; + +class MeasurementModel +{ + private static final DMatrixRMaj eye3x3 = CommonOps_DDRM.identity(3); + + private final StateVariables baseState; + private final StateVariables footState; + private final MeasuredVariables footMeasurements; + private final FrameVector3DReadOnly gravityVector; + + // State variables + public final YoVector3D footPredictedRelativePosition; + public final YoVector3D footPredictedRelativeOrientationError; + public final YoVector3D footPredictedLinearVelocityError; + public final YoVector3D footPredictedLinearVelocity; + public final YoVector3D footPredictedGravityVector; + public final YoQuaternion footOrientationInBaseFrame; + + public final YoBoolean isInContact; + private final DoubleProvider contactThreshold; + + // Temp variables + private final Quaternion footOrientationError = new Quaternion(); + private final Vector3D velocityError = new Vector3D(); + + public MeasurementModel(String prefix, + StateVariables baseState, + StateVariables footState, + MeasuredVariables footMeasurements, + FrameVector3DReadOnly gravityVector, + DoubleProvider contactThreshold, + YoRegistry registry) + { + this.baseState = baseState; + this.footState = footState; + this.footMeasurements = footMeasurements; + this.gravityVector = gravityVector; + this.contactThreshold = contactThreshold; + + footPredictedRelativePosition = new YoVector3D(prefix + "PredictedRelativePosition", registry); + footPredictedRelativeOrientationError = new YoVector3D(prefix + "PredictedRelativeOrientationError", registry); + footPredictedLinearVelocityError = new YoVector3D(prefix + "PredictedLinearVelocityError", registry); + footPredictedLinearVelocity = new YoVector3D(prefix + "PredictedLinearVelocity", registry); + footPredictedGravityVector = new YoVector3D(prefix + "PredictedAccelMeasure", registry); + footOrientationInBaseFrame = new YoQuaternion(prefix + "OrientationInBaseFrame", registry); + isInContact = new YoBoolean(prefix + "IsInContact", registry); + } + + /** + * This is equation 35 + */ + public void update(DMatrixRMaj footCovariance) + { + isInContact.set(isFootInContact(footCovariance)); + + // First row. Update the predicted foot position of the foot relative to the base in the base frame + footPredictedRelativePosition.sub(footState.translation, baseState.translation); + baseState.orientation.transform(footPredictedRelativePosition); // FIXME is this right? transforms do more operations than I think is expected + + // 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(footMeasurements.orientationMeasurement, footOrientationInBaseFrame, footOrientationError); + footOrientationError.getRotationVector(footPredictedRelativeOrientationError); + + // Third row. Compute the linear velocity error in the base frame + velocityError.sub(baseState.linearVelocity, footState.linearVelocity); + baseState.orientation.inverseTransform(velocityError); + + footPredictedLinearVelocityError.cross(baseState.unbiasedGyro, footMeasurements.positionMeasurement); + footPredictedLinearVelocityError.scale(-1.0); + footPredictedLinearVelocityError.sub(velocityError); + footPredictedLinearVelocityError.add(footMeasurements.linearVelocity); + + if (!isInContact.getBooleanValue()) + return; + + // Fourth row. Set the predicted linear velocity + footPredictedLinearVelocity.set(footState.linearVelocity); + + // Fifth row. Set the predicted gravity vector + footState.orientation.transform(gravityVector, footPredictedGravityVector); + if (OdometryKalmanFilter.includeBias) + footPredictedGravityVector.add(footState.accelBias); + } + + private final DMatrixRMaj vector = new DMatrixRMaj(3, 1); + private final DMatrixRMaj tempVector = new DMatrixRMaj(3, 1); + private final DMatrixRMaj tempScalar = new DMatrixRMaj(3, 1); + + private boolean isFootInContact(DMatrixRMaj footCovariance) + { + footState.translation.get(vector); + + CommonOps_DDRM.mult(footCovariance, vector, tempVector); + CommonOps_DDRM.multTransA(vector, tempVector, tempScalar); + + // tests the mahalonobis distance of the foot velocity being below a certain threshold. + return tempScalar.get(0, 0) < contactThreshold.getValue(); + } + + public void get(int start, DMatrixRMaj measurementToPack) + { + footPredictedRelativePosition.get(start, measurementToPack); + footPredictedRelativeOrientationError.get(start + 3, measurementToPack); + footPredictedLinearVelocityError.get(start + 6, measurementToPack); + + if (isInContact.getBooleanValue()) + return; + + footPredictedLinearVelocity.get(start + 9, measurementToPack); + footPredictedGravityVector.get(start + 12, measurementToPack); + } + + /** + * This is equation 41 + */ + public void computeBaseMeasurementJacobian(StateVariables baseState, + StateVariables footState, + MeasuredVariables footMeasurements, + int rowOffset, + DMatrixRMaj jacobianToPack) + { + // FIXME garbage + DMatrixRMaj rotationMatrix = new DMatrixRMaj(3, 3); + DMatrixRMaj rotationMatrixTranspose = new DMatrixRMaj(3, 3); + OdometryTools.toRotationMatrix(baseState.orientation, rotationMatrix); + CommonOps_DDRM.transpose(rotationMatrix, rotationMatrixTranspose); + + Vector3D footPositionError = new Vector3D(); + Vector3D footVelocityError = new Vector3D(); + footPositionError.sub(footState.translation, baseState.translation); + footVelocityError.sub(footState.linearVelocity, baseState.linearVelocity); + baseState.orientation.inverseTransform(footPositionError); + baseState.orientation.inverseTransform(footVelocityError); + + DMatrixRMaj skewMatrix = new DMatrixRMaj(3, 3); + OdometryTools.toSkewSymmetricMatrix(footPositionError, skewMatrix); + + // First row + MatrixTools.setMatrixBlock(jacobianToPack, rowOffset, 0, rotationMatrixTranspose, 0, 0, 3, 3, -1.0); + CommonOps_DDRM.insert(skewMatrix, jacobianToPack, rowOffset, 6); + + // Second row + rowOffset += 3; + DMatrixRMaj left = new DMatrixRMaj(3, 3); + DMatrixRMaj right = new DMatrixRMaj(3, 3); + Quaternion orientation = new Quaternion(footState.orientation); + orientation.multiplyConjugateThis(baseState.orientation); + OdometryKalmanFilter.l3Operator(orientation, left); + OdometryKalmanFilter.r3Operator(footMeasurements.orientationMeasurement, right); + MatrixTools.multAddBlock(-1.0, left, right, jacobianToPack, rowOffset, 6); + + // Third row + rowOffset += 3; + OdometryTools.toSkewSymmetricMatrix(footVelocityError, skewMatrix); + + MatrixTools.setMatrixBlock(jacobianToPack, rowOffset, 3, rotationMatrixTranspose, 0, 0, 3, 3, -1.0); + CommonOps_DDRM.insert(skewMatrix, jacobianToPack, rowOffset, 6); + + if (OdometryKalmanFilter.includeBias) + { + OdometryTools.toSkewSymmetricMatrix(footMeasurements.positionMeasurement, skewMatrix); + MatrixTools.setMatrixBlock(jacobianToPack, rowOffset, 12, skewMatrix, 0, 0, 3, 3, -1.0); + } + } + + /** + * This is equation 42 + */ + public void computeFootMeasurementJacobian(StateVariables baseState, + StateVariables footState, + MeasuredVariables footMeasurements, + FrameVector3DReadOnly gravityVector, + int rowOffset, + int colOffset, + DMatrixRMaj jacobianToPack) + { + // FIXME garbage + DMatrixRMaj rotationMatrix = new DMatrixRMaj(3, 3); + OdometryTools.toRotationMatrix(baseState.orientation, rotationMatrix); + CommonOps_DDRM.transpose(rotationMatrix); + + Vector3D footPositionError = new Vector3D(); + Vector3D footVelocityError = new Vector3D(); + footPositionError.sub(footState.translation, baseState.translation); + footVelocityError.sub(footState.linearVelocity, baseState.linearVelocity); + baseState.orientation.inverseTransform(footPositionError); + baseState.orientation.inverseTransform(footVelocityError); + + DMatrixRMaj skewMatrix = new DMatrixRMaj(3, 3); + OdometryTools.toSkewSymmetricMatrix(footPositionError, skewMatrix); + + // First row + CommonOps_DDRM.insert(rotationMatrix, jacobianToPack, rowOffset, colOffset); + + // Second row + rowOffset += 3; + DMatrixRMaj left = new DMatrixRMaj(3, 3); + Quaternion orientation = new Quaternion(footMeasurements.orientationMeasurement); + orientation.multiplyConjugateOther(baseState.orientation); + orientation.multiply(footState.orientation); + OdometryKalmanFilter.l3Operator(orientation, left); + CommonOps_DDRM.insert(left, jacobianToPack, rowOffset, colOffset + 6); + + // Third row + rowOffset += 3; + CommonOps_DDRM.insert(rotationMatrix, jacobianToPack, rowOffset, colOffset + 3); + + if (isInContact.getBooleanValue()) + return; + + // Fourth row + rowOffset += 3; + CommonOps_DDRM.insert(eye3x3, jacobianToPack, rowOffset, colOffset + 3); + + // Fifth row + rowOffset += 3; + Vector3D gravity = new Vector3D(gravityVector); + footState.orientation.inverseTransform(gravity); + OdometryTools.toSkewSymmetricMatrix(gravityVector, skewMatrix); + CommonOps_DDRM.insert(skewMatrix, jacobianToPack, rowOffset, colOffset + 6); + + if (OdometryKalmanFilter.includeBias) + CommonOps_DDRM.insert(eye3x3, jacobianToPack, rowOffset, colOffset + 9); + } +} 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..617db46f5a41 --- /dev/null +++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/ObservationModel.java @@ -0,0 +1,21 @@ +package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF; + +import org.ejml.data.DMatrixRMaj; +import us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF.OdometryKalmanFilter.MeasuredVariables; + +public class ObservationModel +{ + // State providers + private final MeasuredVariables footMeasurements; + + public ObservationModel(MeasuredVariables footMeasurements) + { + this.footMeasurements = footMeasurements; + } + + public void get(int startRow, DMatrixRMaj observation) + { + footMeasurements.positionMeasurement.get(startRow, observation); + footMeasurements.accelMeasurement.get(startRow + 6, observation); + } +} 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 index bd54ad83d356..db8577b5a8eb 100644 --- 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 @@ -12,23 +12,21 @@ import us.ihmc.euclid.transform.interfaces.RigidBodyTransformReadOnly; 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 us.ihmc.mecano.frames.MovingReferenceFrame; 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.YoVector3D; 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.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; import java.util.ArrayList; @@ -39,6 +37,8 @@ */ public class OdometryKalmanFilter extends ExtendedKalmanFilter { + static final boolean includeBias = false; + // Constants and providers private static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); @@ -60,7 +60,10 @@ public class OdometryKalmanFilter extends ExtendedKalmanFilter private final YoDouble measuredOrientationCovariance = new YoDouble("measuredOrientationCovariance", registry); private final YoDouble velocityErrorCovariance = new YoDouble("velocityErrorCovariance", registry); private final YoDouble contactVelocityCovariance = new YoDouble("contactVelocityCovariance", registry); - private final YoDouble contactOrientationCovariance = new YoDouble("contactOrientationCovariance", registry); + private final YoDouble contactAccelerationCovariance = new YoDouble("contactAccelerationMeasureCovariance", registry); + + private final YoDouble contactThreshold = new YoDouble("contactVarianceThreshold", registry); + // Internal variables for state holding private final YoFrameVector3D yoBaseAngularVelocityMeasurement; @@ -86,7 +89,7 @@ public class OdometryKalmanFilter extends ExtendedKalmanFilter private final List processModels = new ArrayList<>(); private final List measurementModels = new ArrayList<>(); - private final List observations = new ArrayList<>(); + private final List observationModels = new ArrayList<>(); private final YoFrameQuaternion filteredBaseOrientation; private final YoFramePoint3D filteredBaseTranslation; @@ -103,7 +106,6 @@ public class OdometryKalmanFilter extends ExtendedKalmanFilter private final DMatrixRMaj processCovariance; private final DMatrixRMaj measurementCovariance; - private static final DMatrixRMaj eye3x3 = CommonOps_DDRM.identity(3); public OdometryKalmanFilter(IMUSensorReadOnly baseIMU, @@ -116,24 +118,24 @@ public OdometryKalmanFilter(IMUSensorReadOnly baseIMU, { super(OdometryIndexHelper.getStateSizePerLink() * (1 + feetIMUs.size()), OdometryIndexHelper.getObservationSizePerLink() * feetIMUs.size()); - // INitialize covariance + // Initialize covariance processCovariance = new DMatrixRMaj(getStateSize(), getStateSize()); measurementCovariance = new DMatrixRMaj(getMeasurementSize(), getMeasurementSize()); MatrixTools.setDiagonal(processCovariance, 1e-2); MatrixTools.setDiagonal(measurementCovariance, 1e-3); - setProcessCovariance(processCovariance); - setMeasurementCovariance(measurementCovariance); - translationCovariance.set(1e-4); - velocityCovariance.set(1e-3); - orientationCovariance.set(1e-4); - biasCovariance.set(1e-6); + translationCovariance.set(1e-3); + velocityCovariance.set(1e-2); + orientationCovariance.set(1e-2); + biasCovariance.set(1e-4); measuredTranslationCovariance.set(1e-5); - measuredOrientationCovariance.set(1e-5); - velocityErrorCovariance.set(1e-4); + measuredOrientationCovariance.set(1e-2); + velocityErrorCovariance.set(1e-3); contactVelocityCovariance.set(1e-5); - contactOrientationCovariance.set(1e-6); + contactAccelerationCovariance.set(1.0); + + contactThreshold.set(Double.MAX_VALUE); this.baseIMU = baseIMU; this.feetIMUs = feetIMUs; @@ -146,41 +148,37 @@ public OdometryKalmanFilter(IMUSensorReadOnly baseIMU, CMatrix = new DMatrixRMaj(getMeasurementSize(), getStateSize()); observationVector = new DMatrixRMaj(OdometryIndexHelper.getObservationSizePerLink() * feetIMUs.size(), 1); - gravityVector = EuclidFrameFactories.newLinkedFrameVector3DReadOnly(() -> worldFrame, new Vector3D(0, 0, -Math.abs(gravitationalAcceleration))); + gravityVector = EuclidFrameFactories.newLinkedFrameVector3DReadOnly(() -> worldFrame, new Vector3D(0, 0, Math.abs(gravitationalAcceleration))); - baseMeasurement = new MeasuredVariables("base", baseIMU, imuBiasProvider, registry); - baseProcessState = new StateVariables("baseProcessState"); - basePredictedState = new StateVariables("basePredicted"); - processModels.add(new ProcessModel(baseMeasurement, - baseProcessState, + baseMeasurement = new MeasuredVariables("base", baseIMU, baseIMU, imuBiasProvider, registry); + baseProcessState = new StateVariables("baseProcessState", baseIMU.getMeasurementFrame(), registry); + basePredictedState = new StateVariables("basePredicted", baseIMU.getMeasurementFrame(), registry); + processModels.add(new ProcessModel(baseProcessState, basePredictedState, cancelGravityFromAccelerationMeasurement, gravityVector, estimatorDT)); for (int i = 0; i < feetIMUs.size(); i++) { - MeasuredVariables footMeasurements = new MeasuredVariables("foot" + i, feetIMUs.get(i), imuBiasProvider, registry); - StateVariables footProcessState = new StateVariables("foot" + i + "ProcessState"); - StateVariables footPredictedState = new StateVariables("foot" + i + "PredictedState"); + MeasuredVariables footMeasurements = new MeasuredVariables("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); feetMeasurements.add(footMeasurements); feetProcessState.add(footProcessState); feetPredictedState.add(footPredictedState); - observations.add(new Observation("foot" + i + "Observation", baseIMU, feetIMUs.get(i), footMeasurements, registry)); - - processModels.add(new ProcessModel(footMeasurements, - footProcessState, + observationModels.add(new ObservationModel(footMeasurements)); + processModels.add(new ProcessModel(footProcessState, footPredictedState, cancelGravityFromAccelerationMeasurement, gravityVector, estimatorDT)); measurementModels.add(new MeasurementModel("foot" + i, basePredictedState, - baseMeasurement, footPredictedState, - baseIMU, - feetIMUs.get(i), + footMeasurements, gravityVector, + contactThreshold, registry)); } @@ -201,248 +199,59 @@ public OdometryKalmanFilter(IMUSensorReadOnly baseIMU, parentRegistry.addChild(registry); } - private static class ProcessModel + private void initialize() { - // State providers - private final MeasuredVariables measuredVariables; - private final StateVariables currentState; - private final StateVariables predictedState; - private final BooleanProvider cancelGravityFromAccelerationMeasurement; - private final FrameVector3DReadOnly gravityVector; - private final double estimatorDt; + baseProcessState.initialize(); + for (int i = 0; i < feetProcessState.size(); i++) + feetProcessState.get(i).initialize(); - // Temp variables - private final Vector3D unbiasedAcceleration = new Vector3D(); - private final Vector3D unbiasedAngularVelocity = new Vector3D(); - private final Vector3D integratedVelocity = new Vector3D(); - private final Quaternion integratedRotation = new Quaternion(); - - public ProcessModel(MeasuredVariables measuredVariables, - StateVariables processState, - StateVariables predictedState, - BooleanProvider cancelGravityFromAccelerationMeasurement, - FrameVector3DReadOnly gravityVector, - double estimatorDt) + // update the predicted state from the yo variablized state + baseProcessState.get(OdometryIndexHelper.getBasePositionIndex(), state); + for (int i = 0; i < feetIMUs.size(); i++) { - this.measuredVariables = measuredVariables; - this.currentState = processState; - this.predictedState = predictedState; - this.cancelGravityFromAccelerationMeasurement = cancelGravityFromAccelerationMeasurement; - this.gravityVector = gravityVector; - this.estimatorDt = estimatorDt; + feetProcessState.get(i).get(OdometryIndexHelper.getFootPositionIndex(i), state); } - public void update() - { - unbiasedAcceleration.sub(measuredVariables.linearAccelerationMeasurement, currentState.accelBias); - unbiasedAngularVelocity.sub(measuredVariables.gyroMeasurement, currentState.gyroBias); - // transform the acceleration to world - 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, unbiasedAngularVelocity); - OdometryIndexHelper.toQuaternionFromRotationVector(integratedVelocity, integratedRotation); - predictedState.orientation.multiply(currentState.orientation, integratedRotation); - - // Propagate the bias - predictedState.accelBias.set(currentState.accelBias); - predictedState.gyroBias.set(currentState.gyroBias); - } + initializeState(state); } - private static class Observation + static class MeasuredVariables { // State providers private final IMUSensorReadOnly baseIMU; - private final IMUSensorReadOnly footIMU; - private final MeasuredVariables footMeasurements; - - // State variables - private final YoVector3D footPositionFromKinematics; - - // Temp variables - private final FramePoint3D footPosition = new FramePoint3D(); - - public Observation(String prefix, IMUSensorReadOnly baseIMU, IMUSensorReadOnly footIMU, MeasuredVariables footMeasurements, YoRegistry registry) - { - this.baseIMU = baseIMU; - this.footIMU = footIMU; - this.footMeasurements = footMeasurements; - - footPositionFromKinematics = new YoVector3D(prefix + "PositionFromKinematics", registry); - } - - public void update() - { - footPosition.setToZero(footIMU.getMeasurementFrame()); - footPosition.changeFrame(baseIMU.getMeasurementFrame()); - - footPositionFromKinematics.set(footPosition); - } - - public void get(int startRow, DMatrixRMaj observation) - { - footPositionFromKinematics.get(startRow, observation); - footMeasurements.linearAccelerationMeasurement.get(startRow + 6, observation); - } - } - - private static class MeasurementModel - { - private final StateVariables baseState; - private final MeasuredVariables baseMeasurement; - private final StateVariables footState; - private final IMUSensorReadOnly baseIMU; - private final IMUSensorReadOnly footIMU; - private final FrameVector3DReadOnly gravityVector; - - // State variables - public final YoVector3D footPredictedRelativePosition; - public final YoVector3D footPredictedRelativeOrientation; - public final YoVector3D footPredictedLinearVelocityError; - public final YoVector3D footPredictedLinearVelocity; - public final YoVector3D footPredictedGravityVector; - - public final YoBoolean isInContact; - - // Temp variables - private final FramePose3D footPose = new FramePose3D(); - private final Quaternion footOrientationError = new Quaternion(); - private final Vector3D velocityError = new Vector3D(); - private final Vector3D unbiasedAngularVelocity = new Vector3D(); - - public MeasurementModel(String prefix, - StateVariables baseState, - MeasuredVariables baseMeasurement, - StateVariables footState, - IMUSensorReadOnly baseIMU, - IMUSensorReadOnly footIMU, - FrameVector3DReadOnly gravityVector, - YoRegistry registry) - { - this.baseState = baseState; - this.baseMeasurement = baseMeasurement; - this.footState = footState; - this.baseIMU = baseIMU; - this.footIMU = footIMU; - this.gravityVector = gravityVector; - - footPredictedRelativePosition = new YoVector3D(prefix + "PredictedRelativePosition", registry); - footPredictedRelativeOrientation = new YoVector3D(prefix + "PredictedRelativeOrientation", registry); - footPredictedLinearVelocityError = new YoVector3D(prefix + "PredictedLinearVelocityError", registry); - footPredictedLinearVelocity = new YoVector3D(prefix + "PredictedLinearVelocity", registry); - footPredictedGravityVector = new YoVector3D(prefix + "PredictedGravityVector", registry); - isInContact = new YoBoolean(prefix + "IsInContact", registry); - } - - /** - * This is equation 35 - */ - public void update(DMatrixRMaj footCovariance) - { - isInContact.set(isFootInContact(footCovariance)); - - // First row. Update the predicted foot position of the foot relative to the base in the base frame - footPredictedRelativePosition.sub(footState.translation, baseState.translation); - baseState.orientation.transform(footPredictedRelativePosition); - - // Second row. get the foot pose relative to the base link, which should be entirely based on kinematics - footPose.setToZero(footIMU.getMeasurementFrame()); - footPose.changeFrame(baseIMU.getMeasurementFrame()); - - // Update the predicted foot orientation of the foot relative to the base in the base frame - footOrientationError.set(footPose.getOrientation()); - footOrientationError.multiplyConjugateBoth(baseState.orientation); - footOrientationError.multiply(footState.orientation); - OdometryIndexHelper.logMap(footOrientationError, footPredictedRelativeOrientation); - - // Compute the linear velocity error in the base frame - unbiasedAngularVelocity.sub(baseMeasurement.gyroMeasurement, baseState.gyroBias); - velocityError.sub(baseState.linearVelocity, footState.linearVelocity); - baseState.orientation.inverseTransform(velocityError); - - // FIXME this likely changes based on the contact conditions - footPredictedLinearVelocityError.cross(unbiasedAngularVelocity, footPose.getPosition()); - footPredictedLinearVelocityError.scale(-1.0); - footPredictedLinearVelocityError.sub(velocityError); - - // FIXME get the estimated foot linear velocity from the kinematics, and add it to here. That is, get the one using the MovingReferenceFrame, relative to the base IMU. - - if (isInContact.getBooleanValue()) - return; - - // Fourth row. Set the predicted linear velocity - footPredictedLinearVelocity.set(footState.linearVelocity); - - // Fifth row. Set the predicted gravity vector - footState.orientation.transform(gravityVector, footPredictedGravityVector); - footPredictedGravityVector.add(footState.accelBias); - } - - private final DMatrixRMaj vector = new DMatrixRMaj(3, 1); - private final DMatrixRMaj tempVector = new DMatrixRMaj(3, 1); - private final DMatrixRMaj tempScalar = new DMatrixRMaj(3, 1); - - private boolean isFootInContact(DMatrixRMaj footCovariance) - { - footState.translation.get(vector); - - CommonOps_DDRM.mult(footCovariance, vector, tempVector); - CommonOps_DDRM.multTransA(vector, tempVector, tempScalar); - - // tests the mahalonobis distance of the foot velocity being below a certain threshold. - return tempScalar.get(0, 0) < 0.2; // FIXME extract magic number - } - - public void get(int start, DMatrixRMaj measurementToPack) - { - footPredictedRelativePosition.get(start, measurementToPack); - footPredictedRelativeOrientation.get(start + 3, measurementToPack); - footPredictedLinearVelocityError.get(start + 6, measurementToPack); - if (isInContact.getBooleanValue()) - return; - - footPredictedLinearVelocity.get(start + 9, measurementToPack); - footPredictedGravityVector.get(start + 12, measurementToPack); - } - } - - private static class MeasuredVariables - { - // State providers private final IMUSensorReadOnly imu; private final IMUBiasProvider imuBiasProvider; // State recorders public final YoFrameVector3D gyroMeasurementInWorld; public final YoFrameVector3D gyroMeasurement; - public final YoFrameVector3D linearAccelerationMeasurementInWorld; - public final YoFrameVector3D linearAccelerationMeasurement; + public final YoFrameVector3D accelMeasurementInWorld; + public final YoFrameVector3D accelMeasurement; + + public final YoFramePoint3D positionMeasurement; + public final YoFrameQuaternion orientationMeasurement; + public final YoFrameVector3D linearVelocity; // Temp variables private final FrameVector3D linearAcceleration = new FrameVector3D(); private final FrameVector3D angularVelocity = new FrameVector3D(); + private final Twist twist = new Twist(); - public MeasuredVariables(String prefix, IMUSensorReadOnly imu, IMUBiasProvider imuBiasProvider, YoRegistry registry) + public MeasuredVariables(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); - linearAccelerationMeasurementInWorld = new YoFrameVector3D(prefix + "LinearAccelerationMeasurementInWorld", worldFrame, registry); - linearAccelerationMeasurement = new YoFrameVector3D(prefix + "LinearAccelerationMeasurement", 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); } public void update() @@ -464,12 +273,19 @@ public void update() linearAcceleration.setReferenceFrame(imu.getMeasurementFrame()); linearAcceleration.sub(accelRawInput, accelBiasInput); - linearAccelerationMeasurementInWorld.setMatchingFrame(linearAcceleration); - linearAccelerationMeasurement.setMatchingFrame(linearAcceleration); + 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()); } } - private class StateVariables + static class StateVariables { public final YoFramePoint3D translation; public final YoFrameVector3D linearVelocity; @@ -477,22 +293,51 @@ private class StateVariables public final YoFrameVector3D accelBias; public final YoFrameVector3D gyroBias; - public StateVariables(String prefix) + 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", worldFrame, registry); - gyroBias = new YoFrameVector3D(prefix + "GyroBias", 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) + public void set(int start, DMatrixRMaj state, MeasuredVariables measuredVariables) { translation.set(start + OdometryIndexHelper.getStatePositionIndex(), state); linearVelocity.set(start + OdometryIndexHelper.getStateVelocityIndex(), state); orientation.set(start + OdometryIndexHelper.getStateOrientationIndex(), state); accelBias.set(start + OdometryIndexHelper.getStateAccelerationBiasIndex(), state); gyroBias.set(start + OdometryIndexHelper.getStateGyroBiasIndex(), state); + + if (OdometryKalmanFilter.includeBias) + { + unbiasedAccel.sub(measuredVariables.accelMeasurement, accelBias); + unbiasedGyro.sub(measuredVariables.gyroMeasurement, gyroBias); + } + else + { + unbiasedAccel.set(measuredVariables.accelMeasurement); + unbiasedGyro.set(measuredVariables.gyroMeasurement); + } } public void get(int start, DMatrixRMaj stateToPack) @@ -505,6 +350,8 @@ public void get(int start, DMatrixRMaj stateToPack) } } + private boolean initialized = false; + public void compute() { updateCovariance(); @@ -513,11 +360,17 @@ public void compute() for (int i = 0; i < feetMeasurements.size(); i++) feetMeasurements.get(i).update(); - for (int i = 0; i < observations.size(); i++) + for (int i = 0; i < observationModels.size(); i++) + { + observationModels.get(i).get(i * OdometryIndexHelper.getObservationSizePerLink(), observationVector); + } + + if (!initialized) { - observations.get(i).update(); - observations.get(i).get(i * OdometryIndexHelper.getObservationSizePerLink(), observationVector); + initialize(); + initialized = true; } + updateRootJointTwistAngularPart(); // TODO maybe do this after we update everything based on the estimate? DMatrixRMaj stateEstimate = calculateEstimate(observationVector); @@ -561,21 +414,20 @@ private void updateCovariance() for (int i = 0; i < feetIMUs.size(); i++) { int index = i * OdometryIndexHelper.getObservationSizePerLink(); - measurementCovariance.set(index, index++, measuredTranslationCovariance.getValue()); - measurementCovariance.set(index, index++, measuredTranslationCovariance.getValue()); - measurementCovariance.set(index, index++, measuredTranslationCovariance.getValue()); - measurementCovariance.set(index, index++, measuredOrientationCovariance.getValue()); - measurementCovariance.set(index, index++, measuredOrientationCovariance.getValue()); - measurementCovariance.set(index, index++, measuredOrientationCovariance.getValue()); - measurementCovariance.set(index, index++, velocityErrorCovariance.getValue()); - measurementCovariance.set(index, index++, velocityErrorCovariance.getValue()); - measurementCovariance.set(index, index++, velocityErrorCovariance.getValue()); - measurementCovariance.set(index, index++, contactVelocityCovariance.getValue()); - measurementCovariance.set(index, index++, contactVelocityCovariance.getValue()); - measurementCovariance.set(index, index++, contactVelocityCovariance.getValue()); - measurementCovariance.set(index, index++, contactOrientationCovariance.getValue()); - measurementCovariance.set(index, index++, contactOrientationCovariance.getValue()); - measurementCovariance.set(index, index, contactOrientationCovariance.getValue()); + for (int j = 0; j < 3; j++) + measurementCovariance.set(index + j, index + j, measuredTranslationCovariance.getValue()); + index += 3; + for (int j = 0; j < 3; j++) + measurementCovariance.set(index + j, index + j, measuredOrientationCovariance.getValue()); + index += 3; + for (int j = 0; j < 3; j++) + measurementCovariance.set(index + j, index + j, velocityErrorCovariance.getValue()); + index += 3; + for (int j = 0; j < 3; j++) + measurementCovariance.set(index + j, index + j, contactVelocityCovariance.getValue()); + index += 3; + for (int j = 0; j < 3; j++) + measurementCovariance.set(index + j, index + j, contactAccelerationCovariance.getValue()); } setProcessCovariance(processCovariance); @@ -661,10 +513,10 @@ public DMatrixRMaj processModel(DMatrixRMaj state) predictedState.zero(); // update the yo variables representing the state - baseProcessState.set(OdometryIndexHelper.getBasePositionIndex(), state); + baseProcessState.set(OdometryIndexHelper.getBasePositionIndex(), state, baseMeasurement); for (int i = 0; i < feetIMUs.size(); i++) { - feetProcessState.get(i).set(OdometryIndexHelper.getFootPositionIndex(i), state); + feetProcessState.get(i).set(OdometryIndexHelper.getFootPositionIndex(i), state, feetMeasurements.get(i)); } // do the prediction @@ -672,10 +524,10 @@ public DMatrixRMaj processModel(DMatrixRMaj state) processModels.get(i).update(); // update the predicted state from the yo variablized state - basePredictedState.get(OdometryIndexHelper.getBasePositionIndex(), state); + basePredictedState.get(OdometryIndexHelper.getBasePositionIndex(), predictedState); for (int i = 0; i < feetIMUs.size(); i++) { - feetPredictedState.get(i).get(OdometryIndexHelper.getFootPositionIndex(i), state); + feetPredictedState.get(i).get(OdometryIndexHelper.getFootPositionIndex(i), predictedState); } // return the predicted state @@ -706,15 +558,13 @@ 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 rowOffset = 0; - int colOffset = 0; - computeProcessJacobian(baseMeasurement, baseProcessState, estimatorDT, rowOffset, colOffset, AMatrix); + int offset = 0; + ProcessModel.computeProcessJacobian(baseProcessState, estimatorDT, offset, AMatrix); for (int i = 0; i < feetIMUs.size(); i++) { - rowOffset += OdometryIndexHelper.getStateSizePerLink(); - colOffset += OdometryIndexHelper.getStateSizePerLink(); - computeProcessJacobian(feetMeasurements.get(i), feetProcessState.get(i), estimatorDT, rowOffset, colOffset, AMatrix); + offset += OdometryIndexHelper.getStateSizePerLink(); + ProcessModel.computeProcessJacobian(feetProcessState.get(i), estimatorDT, offset, AMatrix); } return AMatrix; @@ -726,12 +576,12 @@ 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 = 0; + int colOffset = OdometryIndexHelper.getStateSizePerLink(); for (int i = 0; i < feetIMUs.size(); i++) { - boolean footIsInContact = measurementModels.get(i).isInContact.getBooleanValue(); - computeBaseMeasurementJacobian(basePredictedState, feetPredictedState.get(i), rowOffset, CMatrix); - computeFootMeasurementJacobian(footIsInContact, basePredictedState, feetPredictedState.get(i), gravityVector, rowOffset, colOffset + 15, CMatrix); + MeasurementModel measurementModel = measurementModels.get(i); + measurementModel.computeBaseMeasurementJacobian(basePredictedState, feetPredictedState.get(i), feetMeasurements.get(i), rowOffset, CMatrix); + measurementModel.computeFootMeasurementJacobian(basePredictedState, feetPredictedState.get(i), feetMeasurements.get(i), gravityVector, rowOffset, colOffset, CMatrix); rowOffset += OdometryIndexHelper.getObservationSizePerLink(); colOffset += OdometryIndexHelper.getStateSizePerLink(); } @@ -739,198 +589,16 @@ protected DMatrixRMaj linearizeMeasurementModel(DMatrixRMaj predictedState) return CMatrix; } - static void toRotationMatrix(QuaternionReadOnly quaternion, DMatrixRMaj rotationMatrix) + static void l3Operator(QuaternionReadOnly quaternion, DMatrixRMaj matrixToPack) { - toRotationMatrix(quaternion.getX(), quaternion.getY(), quaternion.getZ(), quaternion.getS(), rotationMatrix); - } - - 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 toRotationMatrixInverse(QuaternionReadOnly quaternion, DMatrixRMaj rotationMatrix) - { - toRotationMatrixInverse(quaternion.getX(), quaternion.getY(), quaternion.getZ(), quaternion.getS(), rotationMatrix); - } - - 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)); - } - - /** - * This is equation 39 - */ - static void computeProcessJacobian(MeasuredVariables measuredVariables, StateVariables stateVariables, double estimateDT, int rowOffset, int colOffset, DMatrixRMaj jacobianToPack) - { - // Do the partial derivative with respect to the base position state - CommonOps_DDRM.insert(eye3x3, jacobianToPack, rowOffset + OdometryIndexHelper.getStatePositionIndex(), colOffset + OdometryIndexHelper.getStatePositionIndex()); - MatrixTools.setMatrixBlock(jacobianToPack, - rowOffset + OdometryIndexHelper.getStatePositionIndex(), - colOffset + OdometryIndexHelper.getStateVelocityIndex(), - eye3x3, - 0, - 0, - 3, - 3, - estimateDT); - - // Do the partial derivative with respect to the base velocity state FIXME lots of garbage - CommonOps_DDRM.insert(eye3x3, jacobianToPack, rowOffset + OdometryIndexHelper.getStateVelocityIndex(), colOffset + OdometryIndexHelper.getStateVelocityIndex()); - - DMatrixRMaj rotationMatrix = new DMatrixRMaj(3, 3); - DMatrixRMaj skewMatrix = new DMatrixRMaj(3, 3); - Vector3D unbiasedAcceleration = new Vector3D(); - unbiasedAcceleration.sub(measuredVariables.linearAccelerationMeasurement, stateVariables.accelBias); - toRotationMatrix(stateVariables.orientation, rotationMatrix); - toSkewSymmetricMatrix(unbiasedAcceleration, skewMatrix); - MatrixTools.multAddBlock(-estimateDT, rotationMatrix, skewMatrix, jacobianToPack, rowOffset + OdometryIndexHelper.getStateVelocityIndex(), colOffset + OdometryIndexHelper.getStateOrientationIndex()); - - MatrixTools.setMatrixBlock(jacobianToPack, rowOffset + OdometryIndexHelper.getStateVelocityIndex(), colOffset + OdometryIndexHelper.getStateGyroBiasIndex(), rotationMatrix, 0, 0, 3, 3, -estimateDT); - - // Do the partial derivative with respect to the orientation state - Vector3D unbiasedVelocity = new Vector3D(); - unbiasedVelocity.sub(measuredVariables.gyroMeasurement, stateVariables.gyroBias); - toSkewSymmetricMatrix(unbiasedVelocity, skewMatrix); - MatrixTools.setMatrixBlock(jacobianToPack, rowOffset + OdometryIndexHelper.getStateOrientationIndex(), colOffset + OdometryIndexHelper.getStateOrientationIndex(), rotationMatrix, 0, 0, 3, 3, -estimateDT); - MatrixTools.addMatrixBlock(jacobianToPack, rowOffset + OdometryIndexHelper.getStateOrientationIndex(), colOffset + OdometryIndexHelper.getStateOrientationIndex(), eye3x3, 0, 0, 3, 3, 1.0); - MatrixTools.setMatrixBlock(jacobianToPack, rowOffset + OdometryIndexHelper.getStateOrientationIndex(), colOffset + OdometryIndexHelper.getStateAccelerationBiasIndex(), eye3x3, 0, 0, 3, 3, -estimateDT); - - // Do the partial derivative with respect to the acceleration bias state - MatrixTools.setMatrixBlock(jacobianToPack, rowOffset + OdometryIndexHelper.getStateAccelerationBiasIndex(), colOffset + OdometryIndexHelper.getStateAccelerationBiasIndex(), eye3x3, 0, 0, 3, 3, 1.0); - // Do the partial derivative with respect to the gyro bias state - MatrixTools.setMatrixBlock(jacobianToPack, rowOffset + OdometryIndexHelper.getStateGyroBiasIndex(), colOffset + OdometryIndexHelper.getStateGyroBiasIndex(), eye3x3, 0, 0, 3, 3, 1.0); - } - - /** - * This is equation 41 - */ - private void computeBaseMeasurementJacobian(StateVariables baseState, StateVariables footState, int rowOffset, DMatrixRMaj jacobianToPack) - { - DMatrixRMaj rotationMatrix = new DMatrixRMaj(3, 3); - toRotationMatrix(baseState.orientation, rotationMatrix); - CommonOps_DDRM.transpose(rotationMatrix); - - Vector3D footPositionError = new Vector3D(); - Vector3D footVelocityError = new Vector3D(); - footPositionError.sub(footState.translation, baseState.translation); - footVelocityError.sub(footState.linearVelocity, baseState.linearVelocity); - baseState.orientation.inverseTransform(footPositionError); - baseState.orientation.inverseTransform(footVelocityError); - - DMatrixRMaj skewMatrix = new DMatrixRMaj(3, 3); - toSkewSymmetricMatrix(footPositionError, skewMatrix); - - // First row - MatrixTools.setMatrixBlock(jacobianToPack, rowOffset, 0, rotationMatrix, 0, 0, 3, 3, -1.0); - CommonOps_DDRM.insert(skewMatrix, jacobianToPack, rowOffset, 6); - - // FIXME do the second row - - // Third row - toSkewSymmetricMatrix(footVelocityError, skewMatrix); - - MatrixTools.setMatrixBlock(jacobianToPack, rowOffset + 6, 3, rotationMatrix, 0, 0, 3, 3, -1.0); - CommonOps_DDRM.insert(skewMatrix, jacobianToPack, rowOffset+ 6, 6); - // FIXME do the last term, in the fifth column - } - - /** - * This is equation 42 - */ - private void computeFootMeasurementJacobian(boolean isInContact, StateVariables baseState, StateVariables footState, FrameVector3DReadOnly gravityVector, int rowOffset, int colOffset, DMatrixRMaj jacobianToPack) - { - DMatrixRMaj rotationMatrix = new DMatrixRMaj(3, 3); - toRotationMatrix(baseState.orientation, rotationMatrix); - CommonOps_DDRM.transpose(rotationMatrix); - - Vector3D footPositionError = new Vector3D(); - Vector3D footVelocityError = new Vector3D(); - footPositionError.sub(footState.translation, baseState.translation); - footVelocityError.sub(footState.linearVelocity, baseState.linearVelocity); - baseState.orientation.inverseTransform(footPositionError); - baseState.orientation.inverseTransform(footVelocityError); - - DMatrixRMaj skewMatrix = new DMatrixRMaj(3, 3); - toSkewSymmetricMatrix(footPositionError, skewMatrix); - - // First row - CommonOps_DDRM.insert(rotationMatrix, jacobianToPack, rowOffset, colOffset); - - // FIXME do the second row - - // Third row - CommonOps_DDRM.insert(rotationMatrix, jacobianToPack, rowOffset + 6, colOffset + 3); - - if (!isInContact) - return; - - // Fourth row - CommonOps_DDRM.insert(eye3x3, jacobianToPack, rowOffset + 9, colOffset + 3); - - // Fifth row - Vector3D gravity = new Vector3D(gravityVector); - footState.orientation.inverseTransform(gravity); - toSkewSymmetricMatrix(gravityVector, skewMatrix); - CommonOps_DDRM.insert(skewMatrix, jacobianToPack, rowOffset + 12, colOffset + 6); - CommonOps_DDRM.insert(eye3x3, jacobianToPack, rowOffset + 12, colOffset + 9); + OdometryTools.toSkewSymmetricMatrix(quaternion.getX(), quaternion.getY(), quaternion.getZ(), matrixToPack); + CommonOps_DDRM.addEquals(matrixToPack, quaternion.getS(), eye3x3); } - static void toSkewSymmetricMatrix(Vector3DReadOnly vector, DMatrixRMaj matrixToPack) + static void r3Operator(QuaternionReadOnly quaternion, DMatrixRMaj matrixToPack) { - matrixToPack.zero(); - matrixToPack.set(0, 1, -vector.getZ()); - matrixToPack.set(0, 2, vector.getY()); - matrixToPack.set(1, 0, vector.getZ()); - matrixToPack.set(1, 2, -vector.getX()); - matrixToPack.set(2, 0, -vector.getY()); - matrixToPack.set(2, 1, -vector.getX()); + OdometryTools.toSkewSymmetricMatrix(quaternion.getX(), quaternion.getY(), quaternion.getZ(), matrixToPack); + CommonOps_DDRM.scale(-1.0, matrixToPack); + CommonOps_DDRM.addEquals(matrixToPack, quaternion.getS(), eye3x3); } } 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..0cc8fb159d74 --- /dev/null +++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryTools.java @@ -0,0 +1,90 @@ +package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF; + +import org.ejml.data.DMatrixRMaj; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; + +public class OdometryTools +{ + 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(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); + } +} 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..ad068252645a --- /dev/null +++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/ProcessModel.java @@ -0,0 +1,123 @@ +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.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF.OdometryKalmanFilter.StateVariables; +import us.ihmc.yoVariables.providers.BooleanProvider; + +class ProcessModel +{ + private static final DMatrixRMaj eye3x3 = CommonOps_DDRM.identity(3); + + // 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(); + + 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; + } + + 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 static void computeProcessJacobian(StateVariables stateVariables, double estimateDT, int offset, DMatrixRMaj jacobianToPack) + { + // Do the partial derivative with respect to the base position state + int rowStart = offset + OdometryIndexHelper.getStatePositionIndex(); + CommonOps_DDRM.insert(eye3x3, jacobianToPack, rowStart, offset + OdometryIndexHelper.getStatePositionIndex()); + MatrixTools.setMatrixBlock(jacobianToPack, rowStart, offset + OdometryIndexHelper.getStateVelocityIndex(), eye3x3, 0, 0, 3, 3, estimateDT); + + // Do the partial derivative with respect to the base velocity state FIXME lots of garbage + rowStart = offset + OdometryIndexHelper.getStateVelocityIndex(); + CommonOps_DDRM.insert(eye3x3, jacobianToPack, rowStart, offset + OdometryIndexHelper.getStateVelocityIndex()); + + DMatrixRMaj rotationMatrix = new DMatrixRMaj(3, 3); + DMatrixRMaj skewMatrix = new DMatrixRMaj(3, 3); + OdometryTools.toRotationMatrix(stateVariables.orientation, rotationMatrix); + OdometryTools.toSkewSymmetricMatrix(stateVariables.unbiasedAccel, skewMatrix); + MatrixTools.multAddBlock(-estimateDT, rotationMatrix, skewMatrix, jacobianToPack, rowStart, offset + OdometryIndexHelper.getStateOrientationIndex()); + + if (OdometryKalmanFilter.includeBias) + MatrixTools.setMatrixBlock(jacobianToPack, rowStart, offset + OdometryIndexHelper.getStateGyroBiasIndex(), rotationMatrix, 0, 0, 3, 3, -estimateDT); + + // Do the partial derivative with respect to the orientation state + rowStart = offset + OdometryIndexHelper.getStateOrientationIndex(); + OdometryTools.toSkewSymmetricMatrix(stateVariables.unbiasedGyro, skewMatrix); + MatrixTools.setMatrixBlock(jacobianToPack, rowStart, offset + OdometryIndexHelper.getStateOrientationIndex(), rotationMatrix, 0, 0, 3, 3, -estimateDT); + if (OdometryKalmanFilter.includeBias) + { + MatrixTools.addMatrixBlock(jacobianToPack, rowStart, offset + OdometryIndexHelper.getStateOrientationIndex(), eye3x3, 0, 0, 3, 3, 1.0); + MatrixTools.setMatrixBlock(jacobianToPack, rowStart, offset + OdometryIndexHelper.getStateAccelerationBiasIndex(), eye3x3, 0, 0, 3, 3, -estimateDT); + } + + // Do the partial derivative with respect to the acceleration bias state + MatrixTools.setMatrixBlock(jacobianToPack, + offset + OdometryIndexHelper.getStateAccelerationBiasIndex(), + offset + OdometryIndexHelper.getStateAccelerationBiasIndex(), + eye3x3, + 0, + 0, + 3, + 3, + 1.0); + // Do the partial derivative with respect to the gyro bias state + MatrixTools.setMatrixBlock(jacobianToPack, + offset + OdometryIndexHelper.getStateGyroBiasIndex(), + offset + OdometryIndexHelper.getStateGyroBiasIndex(), + eye3x3, + 0, + 0, + 3, + 3, + 1.0); + } +} diff --git a/ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryKalmanFilterTest.java b/ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryKalmanFilterTest.java deleted file mode 100644 index a53fb87195d4..000000000000 --- a/ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryKalmanFilterTest.java +++ /dev/null @@ -1,57 +0,0 @@ -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.tuple4D.Quaternion; - -import java.util.Random; - -public class OdometryKalmanFilterTest -{ - @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); - - OdometryKalmanFilter.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); - - OdometryKalmanFilter.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); - } - } -} 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..76d58f545c32 --- /dev/null +++ b/ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryToolsTest.java @@ -0,0 +1,124 @@ +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.tuple4D.Quaternion; + +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); + } + } +} From a3cda93b44a1969e4e6efa6b1bc0b41c8e701c61 Mon Sep 17 00:00:00 2001 From: Robert Date: Fri, 29 Aug 2025 17:11:23 -0500 Subject: [PATCH 06/18] started doing more cleanup --- .../odomEKF/MeasurementModel.java | 29 +++++++++-------- .../odomEKF/ObservationModel.java | 4 +-- .../odomEKF/OdometryIndexHelper.java | 13 ++++++++ .../odomEKF/OdometryKalmanFilter.java | 31 +++++++++++++------ 4 files changed, 52 insertions(+), 25 deletions(-) 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 index 6aca27f48f58..182c93e374a7 100644 --- 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 @@ -15,6 +15,8 @@ import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; +import static us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF.OdometryIndexHelper.*; + class MeasurementModel { private static final DMatrixRMaj eye3x3 = CommonOps_DDRM.identity(3); @@ -118,15 +120,15 @@ private boolean isFootInContact(DMatrixRMaj footCovariance) public void get(int start, DMatrixRMaj measurementToPack) { - footPredictedRelativePosition.get(start, measurementToPack); - footPredictedRelativeOrientationError.get(start + 3, measurementToPack); - footPredictedLinearVelocityError.get(start + 6, measurementToPack); + footPredictedRelativePosition.get(start + measurementRelativeTranslationIndex, measurementToPack); + footPredictedRelativeOrientationError.get(start + measurementRelativeOrientationErrorIndex, measurementToPack); + footPredictedLinearVelocityError.get(start + measurementRelativeVelocityIndex, measurementToPack); if (isInContact.getBooleanValue()) return; - footPredictedLinearVelocity.get(start + 9, measurementToPack); - footPredictedGravityVector.get(start + 12, measurementToPack); + footPredictedLinearVelocity.get(start + measurementContactVelocityIndex, measurementToPack); + footPredictedGravityVector.get(start + measurementAccelIndex, measurementToPack); } /** @@ -155,30 +157,31 @@ public void computeBaseMeasurementJacobian(StateVariables baseState, OdometryTools.toSkewSymmetricMatrix(footPositionError, skewMatrix); // First row - MatrixTools.setMatrixBlock(jacobianToPack, rowOffset, 0, rotationMatrixTranspose, 0, 0, 3, 3, -1.0); - CommonOps_DDRM.insert(skewMatrix, jacobianToPack, rowOffset, 6); + int row = rowOffset + measurementRelativeTranslationIndex; + MatrixTools.setMatrixBlock(jacobianToPack, row, stateTranslationIndex, rotationMatrixTranspose, 0, 0, 3, 3, -1.0); + CommonOps_DDRM.insert(skewMatrix, jacobianToPack, row, stateOrientationIndex); // Second row - rowOffset += 3; + row = rowOffset + measurementRelativeOrientationErrorIndex; DMatrixRMaj left = new DMatrixRMaj(3, 3); DMatrixRMaj right = new DMatrixRMaj(3, 3); Quaternion orientation = new Quaternion(footState.orientation); orientation.multiplyConjugateThis(baseState.orientation); OdometryKalmanFilter.l3Operator(orientation, left); OdometryKalmanFilter.r3Operator(footMeasurements.orientationMeasurement, right); - MatrixTools.multAddBlock(-1.0, left, right, jacobianToPack, rowOffset, 6); + MatrixTools.multAddBlock(-1.0, left, right, jacobianToPack, row, stateOrientationIndex); // Third row - rowOffset += 3; + row = rowOffset + measurementRelativeVelocityIndex; OdometryTools.toSkewSymmetricMatrix(footVelocityError, skewMatrix); - MatrixTools.setMatrixBlock(jacobianToPack, rowOffset, 3, rotationMatrixTranspose, 0, 0, 3, 3, -1.0); - CommonOps_DDRM.insert(skewMatrix, jacobianToPack, rowOffset, 6); + MatrixTools.setMatrixBlock(jacobianToPack, row, stateLinearVelocityIndex, rotationMatrixTranspose, 0, 0, 3, 3, -1.0); + CommonOps_DDRM.insert(skewMatrix, jacobianToPack, row, stateOrientationIndex); if (OdometryKalmanFilter.includeBias) { OdometryTools.toSkewSymmetricMatrix(footMeasurements.positionMeasurement, skewMatrix); - MatrixTools.setMatrixBlock(jacobianToPack, rowOffset, 12, skewMatrix, 0, 0, 3, 3, -1.0); + MatrixTools.setMatrixBlock(jacobianToPack, row, stateGyroBiasIndex, skewMatrix, 0, 0, 3, 3, -1.0); } } 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 index 617db46f5a41..8cc452e5785c 100644 --- 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 @@ -15,7 +15,7 @@ public ObservationModel(MeasuredVariables footMeasurements) public void get(int startRow, DMatrixRMaj observation) { - footMeasurements.positionMeasurement.get(startRow, observation); - footMeasurements.accelMeasurement.get(startRow + 6, observation); + footMeasurements.positionMeasurement.get(startRow + OdometryIndexHelper.measurementRelativeTranslationIndex, observation); + footMeasurements.accelMeasurement.get(startRow + OdometryIndexHelper.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 index 939b97161e6f..5c5c39d954d9 100644 --- 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 @@ -8,6 +8,19 @@ 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 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 getStateSizePerLink() { return 16; 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 index db8577b5a8eb..3dff3693bff4 100644 --- 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 @@ -124,16 +124,27 @@ public OdometryKalmanFilter(IMUSensorReadOnly baseIMU, MatrixTools.setDiagonal(processCovariance, 1e-2); MatrixTools.setDiagonal(measurementCovariance, 1e-3); - translationCovariance.set(1e-3); - velocityCovariance.set(1e-2); - orientationCovariance.set(1e-2); - biasCovariance.set(1e-4); - - measuredTranslationCovariance.set(1e-5); - measuredOrientationCovariance.set(1e-2); - velocityErrorCovariance.set(1e-3); - contactVelocityCovariance.set(1e-5); - contactAccelerationCovariance.set(1.0); +// translationCovariance.set(1e-3); +// velocityCovariance.set(1e-2); +// orientationCovariance.set(1e-2); +// biasCovariance.set(1e-4); +// +// measuredTranslationCovariance.set(1e-5); +// measuredOrientationCovariance.set(1e-2); +// velocityErrorCovariance.set(1e-3); +// contactVelocityCovariance.set(1e-5); +// contactAccelerationCovariance.set(1.0); + + translationCovariance.set(1e-4); + velocityCovariance.set(1e-4); + orientationCovariance.set(1e-4); + biasCovariance.set(1e-4); + + measuredTranslationCovariance.set(1e-4); + measuredOrientationCovariance.set(1e-4); + velocityErrorCovariance.set(1e-4); + contactVelocityCovariance.set(1e-4); + contactAccelerationCovariance.set(1e-4); contactThreshold.set(Double.MAX_VALUE); From acb972c0d695bd36fd4a28265d6a88577e06b69d Mon Sep 17 00:00:00 2001 From: Robert Date: Sun, 31 Aug 2025 23:55:08 -0500 Subject: [PATCH 07/18] did lots of little bug fixes --- .../ExtendedKalmanFilter.java | 58 ++- .../odomEKF/MeasurementModel.java | 212 +++++------ .../odomEKF/MeasurementResidualVariables.java | 35 ++ .../odomEKF/MeasurementVariables.java | 40 ++ .../odomEKF/ObservationModel.java | 12 +- .../odomEKF/OdometryIndexHelper.java | 139 +------ .../odomEKF/OdometryKalmanFilter.java | 341 ++++++------------ .../odomEKF/OdometryTools.java | 53 +++ .../odomEKF/ProcessModel.java | 68 ++-- .../odomEKF/SensedVariables.java | 91 +++++ .../odomEKF/StateCorrectionVariables.java | 39 ++ .../odomEKF/StateVariables.java | 78 ++++ 12 files changed, 638 insertions(+), 528 deletions(-) create mode 100644 ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/MeasurementResidualVariables.java create mode 100644 ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/MeasurementVariables.java create mode 100644 ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/SensedVariables.java create mode 100644 ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/StateCorrectionVariables.java create mode 100644 ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/StateVariables.java 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 a330c7c34f87..4b65b5f4cad3 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 @@ -82,7 +82,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. */ @@ -108,7 +108,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. */ @@ -145,32 +148,40 @@ public abstract class ExtendedKalmanFilter private final DMatrixRMaj josephGainTermContainer; private final int stateSize; + private final int errorSize; private final int measurementSize; public ExtendedKalmanFilter(int stateSize, int measurementSize) + { + this(stateSize, stateSize, measurementSize); + } + + public ExtendedKalmanFilter(int stateSize, int errorSize, int measurementSize) { this.stateSize = stateSize; + this.errorSize = errorSize; this.measurementSize = measurementSize; - processCovariance = new DMatrixRMaj(stateSize, stateSize); + processCovariance = new DMatrixRMaj(errorSize, errorSize); measurementCovariance = new DMatrixRMaj(measurementSize, measurementSize); - processJacobian = new DMatrixRMaj(stateSize, stateSize); - measurementJacobian = new DMatrixRMaj(measurementSize, stateSize); + processJacobian = new DMatrixRMaj(stateSize, errorSize); + measurementJacobian = 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); + residualCovarianceContainer = new DMatrixRMaj(errorSize, errorSize); residualCovariance = 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); @@ -178,11 +189,11 @@ 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); + josephTransposeTermContainer = new DMatrixRMaj(errorSize, measurementSize); + josephIdentity = CommonOps_DDRM.identity(errorSize); + josephGainTerm = new DMatrixRMaj(errorSize, errorSize); + josephGainTermContainer = new DMatrixRMaj(errorSize, errorSize); } public ExtendedKalmanFilter(DMatrixRMaj initialState, DMatrixRMaj initialCovariance, DMatrixRMaj processCovariance, DMatrixRMaj measurementCovariance) @@ -220,6 +231,11 @@ public int getStateSize() return stateSize; } + public int getErrorSize() + { + return errorSize; + } + public int getMeasurementSize() { return measurementSize; @@ -317,6 +333,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()}. @@ -326,8 +351,8 @@ 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, measurementResidual, stateCorrection); + applyStateCorrection(predictedState, stateCorrection, updatedState); calculateUpdatedCovariance(); state.set(updatedState); @@ -343,6 +368,7 @@ private void calculateResidualCovarianceAndInverse() CommonOps_DDRM.addEquals(residualCovariance, measurementCovariance); solver.setA(residualCovariance); + // fixme this doesn't work. CommonOps_DDRM.invert(residualCovariance, inverseResidualCovariance); // solver.invert(inverseResidualCovariance); } 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 index 182c93e374a7..81b9730322dc 100644 --- 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 @@ -7,10 +7,7 @@ import us.ihmc.euclid.tuple3D.Vector3D; import us.ihmc.euclid.tuple4D.Quaternion; import us.ihmc.matrixlib.MatrixTools; -import us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF.OdometryKalmanFilter.MeasuredVariables; -import us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF.OdometryKalmanFilter.StateVariables; import us.ihmc.yoVariables.euclid.YoQuaternion; -import us.ihmc.yoVariables.euclid.YoVector3D; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; @@ -23,15 +20,11 @@ class MeasurementModel private final StateVariables baseState; private final StateVariables footState; - private final MeasuredVariables footMeasurements; + private final SensedVariables footSensing; + private final MeasurementVariables predictedMeasurements; private final FrameVector3DReadOnly gravityVector; // State variables - public final YoVector3D footPredictedRelativePosition; - public final YoVector3D footPredictedRelativeOrientationError; - public final YoVector3D footPredictedLinearVelocityError; - public final YoVector3D footPredictedLinearVelocity; - public final YoVector3D footPredictedGravityVector; public final YoQuaternion footOrientationInBaseFrame; public final YoBoolean isInContact; @@ -39,27 +32,31 @@ class MeasurementModel // 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 left = new DMatrixRMaj(3, 3); + private final DMatrixRMaj right = new DMatrixRMaj(3, 3); + private final Quaternion tempRotation = new Quaternion(); public MeasurementModel(String prefix, StateVariables baseState, StateVariables footState, - MeasuredVariables footMeasurements, + SensedVariables footSensing, + MeasurementVariables predictedMeasurement, FrameVector3DReadOnly gravityVector, DoubleProvider contactThreshold, YoRegistry registry) { this.baseState = baseState; this.footState = footState; - this.footMeasurements = footMeasurements; + this.footSensing = footSensing; + this.predictedMeasurements = predictedMeasurement; this.gravityVector = gravityVector; this.contactThreshold = contactThreshold; - footPredictedRelativePosition = new YoVector3D(prefix + "PredictedRelativePosition", registry); - footPredictedRelativeOrientationError = new YoVector3D(prefix + "PredictedRelativeOrientationError", registry); - footPredictedLinearVelocityError = new YoVector3D(prefix + "PredictedLinearVelocityError", registry); - footPredictedLinearVelocity = new YoVector3D(prefix + "PredictedLinearVelocity", registry); - footPredictedGravityVector = new YoVector3D(prefix + "PredictedAccelMeasure", registry); footOrientationInBaseFrame = new YoQuaternion(prefix + "OrientationInBaseFrame", registry); isInContact = new YoBoolean(prefix + "IsInContact", registry); } @@ -69,66 +66,58 @@ public MeasurementModel(String prefix, */ public void update(DMatrixRMaj footCovariance) { - isInContact.set(isFootInContact(footCovariance)); + isInContact.set(computeIsFootInContact(footCovariance)); // First row. Update the predicted foot position of the foot relative to the base in the base frame - footPredictedRelativePosition.sub(footState.translation, baseState.translation); - baseState.orientation.transform(footPredictedRelativePosition); // FIXME is this right? transforms do more operations than I think is expected + predictedMeasurements.relativePosition.sub(footState.translation, baseState.translation); + baseState.orientation.transform(predictedMeasurements.relativePosition); // FIXME is this right? transforms do more operations than I think is expected // 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(footMeasurements.orientationMeasurement, footOrientationInBaseFrame, footOrientationError); - footOrientationError.getRotationVector(footPredictedRelativeOrientationError); + 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); - footPredictedLinearVelocityError.cross(baseState.unbiasedGyro, footMeasurements.positionMeasurement); - footPredictedLinearVelocityError.scale(-1.0); - footPredictedLinearVelocityError.sub(velocityError); - footPredictedLinearVelocityError.add(footMeasurements.linearVelocity); + predictedMeasurements.relativeLinearVelocityError.cross(baseState.unbiasedGyro, footSensing.positionMeasurement); + predictedMeasurements.relativeLinearVelocityError.scale(-1.0); + predictedMeasurements.relativeLinearVelocityError.sub(velocityError); + predictedMeasurements.relativeLinearVelocityError.add(footSensing.linearVelocity); if (!isInContact.getBooleanValue()) return; // Fourth row. Set the predicted linear velocity - footPredictedLinearVelocity.set(footState.linearVelocity); + predictedMeasurements.contactVelocity.set(footState.linearVelocity); // Fifth row. Set the predicted gravity vector - footState.orientation.transform(gravityVector, footPredictedGravityVector); + footState.orientation.transform(gravityVector, predictedMeasurements.accelMeasure); if (OdometryKalmanFilter.includeBias) - footPredictedGravityVector.add(footState.accelBias); + predictedMeasurements.accelMeasure.add(footState.accelBias); } private final DMatrixRMaj vector = new DMatrixRMaj(3, 1); - private final DMatrixRMaj tempVector = new DMatrixRMaj(3, 1); + private final DMatrixRMaj tempDVector = new DMatrixRMaj(3, 1); private final DMatrixRMaj tempScalar = new DMatrixRMaj(3, 1); - private boolean isFootInContact(DMatrixRMaj footCovariance) + private boolean computeIsFootInContact(DMatrixRMaj footCovariance) { footState.translation.get(vector); - CommonOps_DDRM.mult(footCovariance, vector, tempVector); - CommonOps_DDRM.multTransA(vector, tempVector, tempScalar); + CommonOps_DDRM.mult(footCovariance, vector, tempDVector); + CommonOps_DDRM.multTransA(vector, tempDVector, tempScalar); // tests the mahalonobis distance of the foot velocity being below a certain threshold. return tempScalar.get(0, 0) < contactThreshold.getValue(); } - public void get(int start, DMatrixRMaj measurementToPack) + public boolean getIsFootInContact() { - footPredictedRelativePosition.get(start + measurementRelativeTranslationIndex, measurementToPack); - footPredictedRelativeOrientationError.get(start + measurementRelativeOrientationErrorIndex, measurementToPack); - footPredictedLinearVelocityError.get(start + measurementRelativeVelocityIndex, measurementToPack); - - if (isInContact.getBooleanValue()) - return; - - footPredictedLinearVelocity.get(start + measurementContactVelocityIndex, measurementToPack); - footPredictedGravityVector.get(start + measurementAccelIndex, measurementToPack); + return isInContact.getBooleanValue(); } /** @@ -136,52 +125,45 @@ public void get(int start, DMatrixRMaj measurementToPack) */ public void computeBaseMeasurementJacobian(StateVariables baseState, StateVariables footState, - MeasuredVariables footMeasurements, + SensedVariables footMeasurements, int rowOffset, DMatrixRMaj jacobianToPack) { - // FIXME garbage - DMatrixRMaj rotationMatrix = new DMatrixRMaj(3, 3); - DMatrixRMaj rotationMatrixTranspose = new DMatrixRMaj(3, 3); - OdometryTools.toRotationMatrix(baseState.orientation, rotationMatrix); - CommonOps_DDRM.transpose(rotationMatrix, rotationMatrixTranspose); - - Vector3D footPositionError = new Vector3D(); - Vector3D footVelocityError = new Vector3D(); - footPositionError.sub(footState.translation, baseState.translation); - footVelocityError.sub(footState.linearVelocity, baseState.linearVelocity); - baseState.orientation.inverseTransform(footPositionError); - baseState.orientation.inverseTransform(footVelocityError); - - DMatrixRMaj skewMatrix = new DMatrixRMaj(3, 3); - OdometryTools.toSkewSymmetricMatrix(footPositionError, skewMatrix); - - // First row + 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, stateTranslationIndex, rotationMatrixTranspose, 0, 0, 3, 3, -1.0); - CommonOps_DDRM.insert(skewMatrix, jacobianToPack, row, stateOrientationIndex); + MatrixTools.setMatrixBlock(jacobianToPack, row, errorTranslationIndex, rotationMatrixTranspose, 0, 0, 3, 3, -1.0); + CommonOps_DDRM.insert(skewMatrix, jacobianToPack, row, errorOrientationIndex); - // Second row + // Second row. Partial of relative foot orientation error w.r.t. the base state row = rowOffset + measurementRelativeOrientationErrorIndex; - DMatrixRMaj left = new DMatrixRMaj(3, 3); - DMatrixRMaj right = new DMatrixRMaj(3, 3); - Quaternion orientation = new Quaternion(footState.orientation); - orientation.multiplyConjugateThis(baseState.orientation); - OdometryKalmanFilter.l3Operator(orientation, left); + tempRotation.set(footState.orientation); + tempRotation.multiplyConjugateThis(baseState.orientation); + OdometryKalmanFilter.l3Operator(tempRotation, left); OdometryKalmanFilter.r3Operator(footMeasurements.orientationMeasurement, right); - MatrixTools.multAddBlock(-1.0, left, right, jacobianToPack, row, stateOrientationIndex); + MatrixTools.multAddBlock(-1.0, left, right, jacobianToPack, row, errorOrientationIndex); - // Third row + // Third row. Partial of relative foot velocity error w.r.t. the base state row = rowOffset + measurementRelativeVelocityIndex; - OdometryTools.toSkewSymmetricMatrix(footVelocityError, skewMatrix); + OdometryTools.toSkewSymmetricMatrix(velocityError, skewMatrix); - MatrixTools.setMatrixBlock(jacobianToPack, row, stateLinearVelocityIndex, rotationMatrixTranspose, 0, 0, 3, 3, -1.0); - CommonOps_DDRM.insert(skewMatrix, jacobianToPack, row, stateOrientationIndex); + MatrixTools.setMatrixBlock(jacobianToPack, row, errorLinearVelocityIndex, rotationMatrixTranspose, 0, 0, 3, 3, -1.0); + CommonOps_DDRM.insert(skewMatrix, jacobianToPack, row, errorOrientationIndex); if (OdometryKalmanFilter.includeBias) { OdometryTools.toSkewSymmetricMatrix(footMeasurements.positionMeasurement, skewMatrix); - MatrixTools.setMatrixBlock(jacobianToPack, row, stateGyroBiasIndex, skewMatrix, 0, 0, 3, 3, -1.0); + MatrixTools.setMatrixBlock(jacobianToPack, row, errorGyroBiasIndex, skewMatrix, 0, 0, 3, 3, -1.0); } } @@ -190,58 +172,54 @@ public void computeBaseMeasurementJacobian(StateVariables baseState, */ public void computeFootMeasurementJacobian(StateVariables baseState, StateVariables footState, - MeasuredVariables footMeasurements, + SensedVariables footMeasurements, FrameVector3DReadOnly gravityVector, int rowOffset, int colOffset, DMatrixRMaj jacobianToPack) { - // FIXME garbage - DMatrixRMaj rotationMatrix = new DMatrixRMaj(3, 3); - OdometryTools.toRotationMatrix(baseState.orientation, rotationMatrix); - CommonOps_DDRM.transpose(rotationMatrix); - - Vector3D footPositionError = new Vector3D(); - Vector3D footVelocityError = new Vector3D(); - footPositionError.sub(footState.translation, baseState.translation); - footVelocityError.sub(footState.linearVelocity, baseState.linearVelocity); - baseState.orientation.inverseTransform(footPositionError); - baseState.orientation.inverseTransform(footVelocityError); - - DMatrixRMaj skewMatrix = new DMatrixRMaj(3, 3); - OdometryTools.toSkewSymmetricMatrix(footPositionError, skewMatrix); - - // First row - CommonOps_DDRM.insert(rotationMatrix, jacobianToPack, rowOffset, colOffset); - - // Second row - rowOffset += 3; - DMatrixRMaj left = new DMatrixRMaj(3, 3); - Quaternion orientation = new Quaternion(footMeasurements.orientationMeasurement); - orientation.multiplyConjugateOther(baseState.orientation); - orientation.multiply(footState.orientation); - OdometryKalmanFilter.l3Operator(orientation, left); - CommonOps_DDRM.insert(left, jacobianToPack, rowOffset, colOffset + 6); - - // Third row - rowOffset += 3; - CommonOps_DDRM.insert(rotationMatrix, jacobianToPack, rowOffset, colOffset + 3); - - if (isInContact.getBooleanValue()) + 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(footMeasurements.orientationMeasurement); + tempRotation.multiplyConjugateOther(baseState.orientation); + tempRotation.multiply(footState.orientation); + OdometryKalmanFilter.l3Operator(tempRotation, left); + CommonOps_DDRM.insert(left, 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); + + if (!isInContact.getBooleanValue()) return; - // Fourth row - rowOffset += 3; - CommonOps_DDRM.insert(eye3x3, jacobianToPack, rowOffset, colOffset + 3); + // 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 - rowOffset += 3; - Vector3D gravity = new Vector3D(gravityVector); - footState.orientation.inverseTransform(gravity); - OdometryTools.toSkewSymmetricMatrix(gravityVector, skewMatrix); - CommonOps_DDRM.insert(skewMatrix, jacobianToPack, rowOffset, colOffset + 6); + // 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, rowOffset, colOffset + 9); + 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..8c579577c615 --- /dev/null +++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/MeasurementVariables.java @@ -0,0 +1,40 @@ +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.*; +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(boolean isInContact, int start, DMatrixRMaj measurementToPack) + { + relativePosition.get(start + measurementRelativeTranslationIndex, measurementToPack); + relativeOrientationError.get(start + measurementRelativeOrientationErrorIndex, measurementToPack); + relativeLinearVelocityError.get(start + measurementRelativeVelocityIndex, measurementToPack); + + if (!isInContact) + return; + + 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 index 8cc452e5785c..220f63eca37a 100644 --- 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 @@ -1,21 +1,23 @@ package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF; import org.ejml.data.DMatrixRMaj; -import us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF.OdometryKalmanFilter.MeasuredVariables; + +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 MeasuredVariables footMeasurements; + private final SensedVariables footMeasurements; - public ObservationModel(MeasuredVariables footMeasurements) + public ObservationModel(SensedVariables footMeasurements) { this.footMeasurements = footMeasurements; } public void get(int startRow, DMatrixRMaj observation) { - footMeasurements.positionMeasurement.get(startRow + OdometryIndexHelper.measurementRelativeTranslationIndex, observation); - footMeasurements.accelMeasurement.get(startRow + OdometryIndexHelper.measurementAccelIndex, 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 index 5c5c39d954d9..ae1222f2e200 100644 --- 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 @@ -1,11 +1,5 @@ package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF; -import us.ihmc.euclid.tools.EuclidCoreTools; -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; - public class OdometryIndexHelper { static final int stateSizePerLink = 16; @@ -15,137 +9,22 @@ public class OdometryIndexHelper 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 getStateSizePerLink() - { - return 16; - } - - public static int getObservationSizePerLink() - { - return 15; - } - - public static int getStatePositionIndex() - { - return 0; - } - - public static int getStateVelocityIndex() - { - return 3; - } - - public static int getStateOrientationIndex() - { - return 6; - } - - public static int getStateAccelerationBiasIndex() - { - return 10; - } - - public static int getStateGyroBiasIndex() - { - return 13; - } - - public static int getBasePositionIndex() - { - return getStatePositionIndex(); - } - - public static int getBaseVelocityIndex() - { - return getStateVelocityIndex(); - } - - public static int getBaseOrientationIndex() - { - return getStateOrientationIndex(); - } - - public static int getBaseAccelerationBiasIndex() - { - return getStateAccelerationBiasIndex(); - } - - public static int getBaseGyroBiasIndex() - { - return getStateGyroBiasIndex(); - } - public static int getFootPositionIndex(int footNumber) { - return getStateSizePerLink() * (footNumber + 1) + getStatePositionIndex(); - } - - public static int getFootVelocityIndex(int footNumber) - { - return getFootPositionIndex(footNumber) + getStateVelocityIndex(); - } - - public static int getFootOrientationIndex(int footNumber) - { - return getFootPositionIndex(footNumber) + getStateOrientationIndex(); - } - - public static int getFootAccelerationBiasIndex(int footNumber) - { - return getFootPositionIndex(footNumber) + getStateAccelerationBiasIndex(); - } - - public static int getFootGyroBiasIndex(int footNumber) - { - return getFootPositionIndex(footNumber) + getStateGyroBiasIndex(); - } - - public static void toQuaternionFromRotationVector(Vector3DReadOnly rotation, QuaternionBasics quaternionToPack) - { - double magnitude = rotation.norm(); - if (magnitude < 1e-7) - { - quaternionToPack.setToZero(); - return; - } - - double s = Math.sin(magnitude * 0.5); - double qs = Math.cos(magnitude * 0.5); - 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 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); + 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 index 3dff3693bff4..c05fbd310876 100644 --- 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 @@ -11,10 +11,9 @@ import us.ihmc.euclid.referenceFrame.tools.EuclidFrameFactories; import us.ihmc.euclid.transform.interfaces.RigidBodyTransformReadOnly; 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 us.ihmc.mecano.frames.MovingReferenceFrame; import us.ihmc.mecano.multiBodySystem.interfaces.FloatingJointBasics; import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; import us.ihmc.mecano.spatial.Twist; @@ -32,6 +31,8 @@ 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 */ @@ -64,7 +65,6 @@ public class OdometryKalmanFilter extends ExtendedKalmanFilter private final YoDouble contactThreshold = new YoDouble("contactVarianceThreshold", registry); - // Internal variables for state holding private final YoFrameVector3D yoBaseAngularVelocityMeasurement; @@ -79,13 +79,17 @@ public class OdometryKalmanFilter extends ExtendedKalmanFilter private final DMatrixRMaj AMatrix; private final DMatrixRMaj CMatrix; - private final MeasuredVariables baseMeasurement; - private final List feetMeasurements = new ArrayList<>(); + 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<>(); @@ -116,35 +120,36 @@ public OdometryKalmanFilter(IMUSensorReadOnly baseIMU, double gravitationalAcceleration, YoRegistry parentRegistry) { - super(OdometryIndexHelper.getStateSizePerLink() * (1 + feetIMUs.size()), OdometryIndexHelper.getObservationSizePerLink() * feetIMUs.size()); + super(stateSizePerLink * (1 + feetIMUs.size()), errorSizePerLink * (1 + feetIMUs.size()) , measurementSizePerLink * feetIMUs.size()); + int links = 1 + feetIMUs.size(); // Initialize covariance - processCovariance = new DMatrixRMaj(getStateSize(), getStateSize()); + processCovariance = new DMatrixRMaj(getErrorSize(), getErrorSize()); measurementCovariance = new DMatrixRMaj(getMeasurementSize(), getMeasurementSize()); MatrixTools.setDiagonal(processCovariance, 1e-2); MatrixTools.setDiagonal(measurementCovariance, 1e-3); -// translationCovariance.set(1e-3); -// velocityCovariance.set(1e-2); -// orientationCovariance.set(1e-2); -// biasCovariance.set(1e-4); + translationCovariance.set(1e-3); + velocityCovariance.set(1e-2); + orientationCovariance.set(1e-2); + biasCovariance.set(1e-4); + + measuredTranslationCovariance.set(1e-5); + measuredOrientationCovariance.set(1e-2); + velocityErrorCovariance.set(1e-3); + contactVelocityCovariance.set(1e-5); + contactAccelerationCovariance.set(1.0); + +// translationCovariance.set(1e-4); +// velocityCovariance.set(1e-4); +// orientationCovariance.set(1e-4); +// biasCovariance.set(1e-4); // -// measuredTranslationCovariance.set(1e-5); -// measuredOrientationCovariance.set(1e-2); -// velocityErrorCovariance.set(1e-3); -// contactVelocityCovariance.set(1e-5); -// contactAccelerationCovariance.set(1.0); - - translationCovariance.set(1e-4); - velocityCovariance.set(1e-4); - orientationCovariance.set(1e-4); - biasCovariance.set(1e-4); - - measuredTranslationCovariance.set(1e-4); - measuredOrientationCovariance.set(1e-4); - velocityErrorCovariance.set(1e-4); - contactVelocityCovariance.set(1e-4); - contactAccelerationCovariance.set(1e-4); +// measuredTranslationCovariance.set(1e-4); +// measuredOrientationCovariance.set(1e-4); +// velocityErrorCovariance.set(1e-4); +// contactVelocityCovariance.set(1e-4); +// contactAccelerationCovariance.set(1e-4); contactThreshold.set(Double.MAX_VALUE); @@ -155,15 +160,16 @@ public OdometryKalmanFilter(IMUSensorReadOnly baseIMU, predictedState = new DMatrixRMaj(getStateSize(), 1); predictedMeasurement = new DMatrixRMaj(getMeasurementSize(), 1); - AMatrix = new DMatrixRMaj(getStateSize(), getStateSize()); - CMatrix = new DMatrixRMaj(getMeasurementSize(), getStateSize()); - observationVector = new DMatrixRMaj(OdometryIndexHelper.getObservationSizePerLink() * feetIMUs.size(), 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))); - baseMeasurement = new MeasuredVariables("base", baseIMU, baseIMU, imuBiasProvider, registry); + 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, @@ -171,14 +177,20 @@ public OdometryKalmanFilter(IMUSensorReadOnly baseIMU, estimatorDT)); for (int i = 0; i < feetIMUs.size(); i++) { - MeasuredVariables footMeasurements = new MeasuredVariables("foot" + i, baseIMU, feetIMUs.get(i), imuBiasProvider, registry); + 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); - feetMeasurements.add(footMeasurements); + 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(footMeasurements)); + observationModels.add(new ObservationModel(footSensing)); processModels.add(new ProcessModel(footProcessState, footPredictedState, cancelGravityFromAccelerationMeasurement, @@ -187,7 +199,8 @@ public OdometryKalmanFilter(IMUSensorReadOnly baseIMU, measurementModels.add(new MeasurementModel("foot" + i, basePredictedState, footPredictedState, - footMeasurements, + footSensing, + footMeasurement, gravityVector, contactThreshold, registry)); @@ -217,7 +230,7 @@ private void initialize() feetProcessState.get(i).initialize(); // update the predicted state from the yo variablized state - baseProcessState.get(OdometryIndexHelper.getBasePositionIndex(), state); + baseProcessState.get(stateTranslationIndex, state); for (int i = 0; i < feetIMUs.size(); i++) { feetProcessState.get(i).get(OdometryIndexHelper.getFootPositionIndex(i), state); @@ -226,154 +239,19 @@ private void initialize() initializeState(state); } - static class MeasuredVariables - { - // 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; - - // Temp variables - private final FrameVector3D linearAcceleration = new FrameVector3D(); - private final FrameVector3D angularVelocity = new FrameVector3D(); - private final Twist twist = new Twist(); - - public MeasuredVariables(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); - } - - 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()); - } - } - - static class StateVariables - { - 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, MeasuredVariables measuredVariables) - { - translation.set(start + OdometryIndexHelper.getStatePositionIndex(), state); - linearVelocity.set(start + OdometryIndexHelper.getStateVelocityIndex(), state); - orientation.set(start + OdometryIndexHelper.getStateOrientationIndex(), state); - accelBias.set(start + OdometryIndexHelper.getStateAccelerationBiasIndex(), state); - gyroBias.set(start + OdometryIndexHelper.getStateGyroBiasIndex(), state); - - if (OdometryKalmanFilter.includeBias) - { - unbiasedAccel.sub(measuredVariables.accelMeasurement, accelBias); - unbiasedGyro.sub(measuredVariables.gyroMeasurement, gyroBias); - } - else - { - unbiasedAccel.set(measuredVariables.accelMeasurement); - unbiasedGyro.set(measuredVariables.gyroMeasurement); - } - } - - public void get(int start, DMatrixRMaj stateToPack) - { - translation.get(start + OdometryIndexHelper.getStatePositionIndex(), stateToPack); - linearVelocity.get(start + OdometryIndexHelper.getStateVelocityIndex(), stateToPack); - orientation.get(start + OdometryIndexHelper.getStateOrientationIndex(), stateToPack); - accelBias.get(start + OdometryIndexHelper.getStateAccelerationBiasIndex(), stateToPack); - gyroBias.get(start + OdometryIndexHelper.getStateGyroBiasIndex(), stateToPack); - } - } - private boolean initialized = false; public void compute() { updateCovariance(); - baseMeasurement.update(); - for (int i = 0; i < feetMeasurements.size(); i++) - feetMeasurements.get(i).update(); + 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 * OdometryIndexHelper.getObservationSizePerLink(), observationVector); + observationModels.get(i).get(i * measurementSizePerLink, observationVector); } if (!initialized) @@ -403,42 +281,22 @@ private void updateCovariance() { for (int i = 0; i < feetIMUs.size() + 1; i++) { - int index = i * OdometryIndexHelper.getStateSizePerLink(); - processCovariance.set(index, index++, translationCovariance.getValue()); - processCovariance.set(index, index++, translationCovariance.getValue()); - processCovariance.set(index, index++, translationCovariance.getValue()); - processCovariance.set(index, index++, velocityCovariance.getValue()); - processCovariance.set(index, index++, velocityCovariance.getValue()); - processCovariance.set(index, index++, velocityCovariance.getValue()); - processCovariance.set(index, index++, orientationCovariance.getValue()); - processCovariance.set(index, index++, orientationCovariance.getValue()); - processCovariance.set(index, index++, orientationCovariance.getValue()); - processCovariance.set(index, index++, orientationCovariance.getValue()); - processCovariance.set(index, index++, biasCovariance.getValue()); - processCovariance.set(index, index++, biasCovariance.getValue()); - processCovariance.set(index, index++, biasCovariance.getValue()); - processCovariance.set(index, index++, biasCovariance.getValue()); - processCovariance.set(index, index++, biasCovariance.getValue()); - processCovariance.set(index, index, biasCovariance.getValue()); + int index = i * errorSizePerLink; + OdometryTools.setDiagonals(index + errorTranslationIndex, 3, translationCovariance.getValue(), processCovariance); + OdometryTools.setDiagonals(index + errorLinearVelocityIndex, 3, velocityCovariance.getValue(), processCovariance); + OdometryTools.setDiagonals(index + errorOrientationIndex, 3, orientationCovariance.getValue(), processCovariance); + OdometryTools.setDiagonals(index + errorAccelBiasIndex, 3, biasCovariance.getValue(), processCovariance); + OdometryTools.setDiagonals(index + errorGyroBiasIndex, 3, biasCovariance.getValue(), processCovariance); } for (int i = 0; i < feetIMUs.size(); i++) { - int index = i * OdometryIndexHelper.getObservationSizePerLink(); - for (int j = 0; j < 3; j++) - measurementCovariance.set(index + j, index + j, measuredTranslationCovariance.getValue()); - index += 3; - for (int j = 0; j < 3; j++) - measurementCovariance.set(index + j, index + j, measuredOrientationCovariance.getValue()); - index += 3; - for (int j = 0; j < 3; j++) - measurementCovariance.set(index + j, index + j, velocityErrorCovariance.getValue()); - index += 3; - for (int j = 0; j < 3; j++) - measurementCovariance.set(index + j, index + j, contactVelocityCovariance.getValue()); - index += 3; - for (int j = 0; j < 3; j++) - measurementCovariance.set(index + j, index + j, contactAccelerationCovariance.getValue()); + int index = i * measurementSizePerLink; + OdometryTools.setDiagonals(index + measurementRelativeTranslationIndex, 3, measuredTranslationCovariance.getValue(), measurementCovariance); + OdometryTools.setDiagonals(index + measurementRelativeOrientationErrorIndex, 3, measuredOrientationCovariance.getValue(), measurementCovariance); + OdometryTools.setDiagonals(index + measurementRelativeVelocityIndex, 3, velocityErrorCovariance.getValue(), measurementCovariance); + OdometryTools.setDiagonals(index + measurementContactVelocityIndex, 3, contactVelocityCovariance.getValue(), measurementCovariance); + OdometryTools.setDiagonals(index + measurementAccelIndex, 3, contactAccelerationCovariance.getValue(), measurementCovariance); } setProcessCovariance(processCovariance); @@ -496,9 +354,9 @@ private void updateRootJointTwistAngularPart() } 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) @@ -518,16 +376,56 @@ private void updateRootJointTwistLinearPart(FrameVector3DReadOnly imuLinearVeloc 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(OdometryIndexHelper.getBasePositionIndex(), state, baseMeasurement); + baseProcessState.set(stateTranslationIndex, state, baseSensing); for (int i = 0; i < feetIMUs.size(); i++) { - feetProcessState.get(i).set(OdometryIndexHelper.getFootPositionIndex(i), state, feetMeasurements.get(i)); + feetProcessState.get(i).set(OdometryIndexHelper.getFootPositionIndex(i), state, feetSensing.get(i)); } // do the prediction @@ -535,7 +433,7 @@ public DMatrixRMaj processModel(DMatrixRMaj state) processModels.get(i).update(); // update the predicted state from the yo variablized state - basePredictedState.get(OdometryIndexHelper.getBasePositionIndex(), predictedState); + basePredictedState.get(stateTranslationIndex, predictedState); for (int i = 0; i < feetIMUs.size(); i++) { feetPredictedState.get(i).get(OdometryIndexHelper.getFootPositionIndex(i), predictedState); @@ -553,11 +451,11 @@ public DMatrixRMaj measurementModel(DMatrixRMaj predictedState) // 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 = OdometryIndexHelper.getFootVelocityIndex(i); + int row = errorSizePerLink * (i + 1) + errorLinearVelocityIndex; MatrixTools.setMatrixBlock(footCovariance, 0, 0, getCovariance(), row, row, 3, 3, 1.0); measurementModels.get(i).update(footCovariance); - measurementModels.get(i).get(i * OdometryIndexHelper.getObservationSizePerLink(), predictedMeasurement); + feetMeasurements.get(i).get(measurementModels.get(i).getIsFootInContact(), i * measurementSizePerLink, predictedMeasurement); } // return the predicted measurement @@ -570,12 +468,11 @@ 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; - ProcessModel.computeProcessJacobian(baseProcessState, estimatorDT, offset, AMatrix); - for (int i = 0; i < feetIMUs.size(); i++) + for (int i = 0; i < feetIMUs.size() + 1; i++) { - offset += OdometryIndexHelper.getStateSizePerLink(); - ProcessModel.computeProcessJacobian(feetProcessState.get(i), estimatorDT, offset, AMatrix); + processModels.get(i).computeProcessJacobian(baseProcessState, estimatorDT, offset, AMatrix); + offset += errorSizePerLink; } return AMatrix; @@ -587,14 +484,14 @@ 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 = OdometryIndexHelper.getStateSizePerLink(); + int colOffset = errorSizePerLink; for (int i = 0; i < feetIMUs.size(); i++) { MeasurementModel measurementModel = measurementModels.get(i); - measurementModel.computeBaseMeasurementJacobian(basePredictedState, feetPredictedState.get(i), feetMeasurements.get(i), rowOffset, CMatrix); - measurementModel.computeFootMeasurementJacobian(basePredictedState, feetPredictedState.get(i), feetMeasurements.get(i), gravityVector, rowOffset, colOffset, CMatrix); - rowOffset += OdometryIndexHelper.getObservationSizePerLink(); - colOffset += OdometryIndexHelper.getStateSizePerLink(); + measurementModel.computeBaseMeasurementJacobian(basePredictedState, feetPredictedState.get(i), feetSensing.get(i), rowOffset, CMatrix); + measurementModel.computeFootMeasurementJacobian(basePredictedState, feetPredictedState.get(i), feetSensing.get(i), gravityVector, rowOffset, colOffset, CMatrix); + rowOffset += measurementSizePerLink; + colOffset += errorSizePerLink; } return CMatrix; 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 index 0cc8fb159d74..fcdc140d3ea9 100644 --- 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 @@ -1,7 +1,11 @@ package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF; import org.ejml.data.DMatrixRMaj; +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; public class OdometryTools @@ -87,4 +91,53 @@ static void toRotationMatrix(QuaternionReadOnly quaternion, DMatrixRMaj rotation { 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 toQuaternionFromRotationVector(Vector3DReadOnly rotation, QuaternionBasics quaternionToPack) + { + double magnitude = rotation.norm(); + if (magnitude < 1e-7) + { + quaternionToPack.setToZero(); + return; + } + + double s = Math.sin(magnitude * 0.5); + double qs = Math.cos(magnitude * 0.5); + 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 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); + } } 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 index ad068252645a..754597fd6aaf 100644 --- 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 @@ -7,12 +7,14 @@ import us.ihmc.euclid.tuple3D.Vector3D; import us.ihmc.euclid.tuple4D.Quaternion; import us.ihmc.matrixlib.MatrixTools; -import us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF.OdometryKalmanFilter.StateVariables; 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; // State providers private final StateVariables currentState; @@ -25,6 +27,9 @@ class ProcessModel 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, @@ -37,6 +42,9 @@ public ProcessModel(StateVariables processState, this.cancelGravityFromAccelerationMeasurement = cancelGravityFromAccelerationMeasurement; this.gravityVector = gravityVector; this.estimatorDt = estimatorDt; + + deltaT = new DMatrixRMaj(eye3x3); + CommonOps_DDRM.scale(estimatorDt, deltaT); } public void update() @@ -69,55 +77,39 @@ public void update() /** * This is equation 39 */ - public static void computeProcessJacobian(StateVariables stateVariables, double estimateDT, int offset, DMatrixRMaj jacobianToPack) + public void computeProcessJacobian(StateVariables stateVariables, double estimateDT, int offset, DMatrixRMaj jacobianToPack) { - // Do the partial derivative with respect to the base position state - int rowStart = offset + OdometryIndexHelper.getStatePositionIndex(); - CommonOps_DDRM.insert(eye3x3, jacobianToPack, rowStart, offset + OdometryIndexHelper.getStatePositionIndex()); - MatrixTools.setMatrixBlock(jacobianToPack, rowStart, offset + OdometryIndexHelper.getStateVelocityIndex(), eye3x3, 0, 0, 3, 3, estimateDT); + // 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); - // Do the partial derivative with respect to the base velocity state FIXME lots of garbage - rowStart = offset + OdometryIndexHelper.getStateVelocityIndex(); - CommonOps_DDRM.insert(eye3x3, jacobianToPack, rowStart, offset + OdometryIndexHelper.getStateVelocityIndex()); + // 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); - DMatrixRMaj rotationMatrix = new DMatrixRMaj(3, 3); - DMatrixRMaj skewMatrix = new DMatrixRMaj(3, 3); OdometryTools.toRotationMatrix(stateVariables.orientation, rotationMatrix); OdometryTools.toSkewSymmetricMatrix(stateVariables.unbiasedAccel, skewMatrix); - MatrixTools.multAddBlock(-estimateDT, rotationMatrix, skewMatrix, jacobianToPack, rowStart, offset + OdometryIndexHelper.getStateOrientationIndex()); + MatrixTools.multAddBlock(-estimateDT, rotationMatrix, skewMatrix, jacobianToPack, rowStart, offset + errorOrientationIndex); if (OdometryKalmanFilter.includeBias) - MatrixTools.setMatrixBlock(jacobianToPack, rowStart, offset + OdometryIndexHelper.getStateGyroBiasIndex(), rotationMatrix, 0, 0, 3, 3, -estimateDT); + MatrixTools.setMatrixBlock(jacobianToPack, rowStart, offset + errorAccelBiasIndex, rotationMatrix, 0, 0, 3, 3, -estimateDT); - // Do the partial derivative with respect to the orientation state - rowStart = offset + OdometryIndexHelper.getStateOrientationIndex(); + // Third row. Partial derivative with respect to the orientation state + rowStart = offset + errorOrientationIndex; OdometryTools.toSkewSymmetricMatrix(stateVariables.unbiasedGyro, skewMatrix); - MatrixTools.setMatrixBlock(jacobianToPack, rowStart, offset + OdometryIndexHelper.getStateOrientationIndex(), rotationMatrix, 0, 0, 3, 3, -estimateDT); + CommonOps_DDRM.insert(eye3x3, jacobianToPack, rowStart, offset + errorOrientationIndex); + MatrixTools.addMatrixBlock(jacobianToPack, rowStart, offset + errorOrientationIndex, skewMatrix, 0, 0, 3, 3, -estimateDT); if (OdometryKalmanFilter.includeBias) { - MatrixTools.addMatrixBlock(jacobianToPack, rowStart, offset + OdometryIndexHelper.getStateOrientationIndex(), eye3x3, 0, 0, 3, 3, 1.0); - MatrixTools.setMatrixBlock(jacobianToPack, rowStart, offset + OdometryIndexHelper.getStateAccelerationBiasIndex(), eye3x3, 0, 0, 3, 3, -estimateDT); + MatrixTools.setMatrixBlock(jacobianToPack, rowStart, offset + errorGyroBiasIndex, deltaT, 0, 0, 3, 3, -1.0); } - // Do the partial derivative with respect to the acceleration bias state - MatrixTools.setMatrixBlock(jacobianToPack, - offset + OdometryIndexHelper.getStateAccelerationBiasIndex(), - offset + OdometryIndexHelper.getStateAccelerationBiasIndex(), - eye3x3, - 0, - 0, - 3, - 3, - 1.0); - // Do the partial derivative with respect to the gyro bias state - MatrixTools.setMatrixBlock(jacobianToPack, - offset + OdometryIndexHelper.getStateGyroBiasIndex(), - offset + OdometryIndexHelper.getStateGyroBiasIndex(), - eye3x3, - 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..7d38a65007e5 --- /dev/null +++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/SensedVariables.java @@ -0,0 +1,91 @@ +package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF; + +import us.ihmc.euclid.referenceFrame.FramePoint3D; +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; + +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; + + // 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); + } + + 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()); + + FramePoint3D imuInWorld = new FramePoint3D(imu.getMeasurementFrame()); + FramePoint3D baseInWorld = new FramePoint3D(baseIMU.getMeasurementFrame()); + imuInWorld.changeFrame(worldFrame); + baseInWorld.changeFrame(worldFrame); + + 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..1980cc940f47 --- /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 measuredVariables) + { + 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(measuredVariables.accelMeasurement, accelBias); + unbiasedGyro.sub(measuredVariables.gyroMeasurement, gyroBias); + } + else + { + unbiasedAccel.set(measuredVariables.accelMeasurement); + unbiasedGyro.set(measuredVariables.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); + } +} From 63038d2dcb5aee31a66d79b1bab216ae66a02a6c Mon Sep 17 00:00:00 2001 From: Robert Date: Mon, 1 Sep 2025 23:22:42 -0500 Subject: [PATCH 08/18] did a lot more work on writing some tests, which uncovered some bugs --- .../odomEKF/MeasurementModel.java | 39 +- .../odomEKF/OdometryKalmanFilter.java | 21 +- .../odomEKF/OdometryTools.java | 152 ++++++++ .../odomEKF/ProcessModel.java | 13 +- .../odomEKF/StateVariables.java | 10 +- .../odomEKF/MeasurementModelTest.java | 325 ++++++++++++++++ .../odomEKF/OdomTestTools.java | 48 +++ .../odomEKF/OdometryToolsTest.java | 1 + .../odomEKF/ProcessModelTest.java | 347 ++++++++++++++++++ 9 files changed, 907 insertions(+), 49 deletions(-) create mode 100644 ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/MeasurementModelTest.java create mode 100644 ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdomTestTools.java create mode 100644 ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/ProcessModelTest.java 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 index 81b9730322dc..45473b51ac43 100644 --- 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 @@ -37,8 +37,11 @@ class MeasurementModel 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 left = new DMatrixRMaj(3, 3); - private final DMatrixRMaj right = 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 DMatrixRMaj lr3x3 = new DMatrixRMaj(3, 3); private final Quaternion tempRotation = new Quaternion(); public MeasurementModel(String prefix, @@ -123,11 +126,7 @@ public boolean getIsFootInContact() /** * This is equation 41 */ - public void computeBaseMeasurementJacobian(StateVariables baseState, - StateVariables footState, - SensedVariables footMeasurements, - int rowOffset, - DMatrixRMaj jacobianToPack) + public void computeBaseMeasurementJacobian(int rowOffset, DMatrixRMaj jacobianToPack) { OdometryTools.toRotationMatrix(baseState.orientation, rotationMatrixTranspose); CommonOps_DDRM.transpose(rotationMatrixTranspose); @@ -147,11 +146,11 @@ public void computeBaseMeasurementJacobian(StateVariables baseState, // Second row. Partial of relative foot orientation error w.r.t. the base state row = rowOffset + measurementRelativeOrientationErrorIndex; - tempRotation.set(footState.orientation); - tempRotation.multiplyConjugateThis(baseState.orientation); - OdometryKalmanFilter.l3Operator(tempRotation, left); - OdometryKalmanFilter.r3Operator(footMeasurements.orientationMeasurement, right); - MatrixTools.multAddBlock(-1.0, left, right, jacobianToPack, row, errorOrientationIndex); + 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; @@ -162,7 +161,7 @@ public void computeBaseMeasurementJacobian(StateVariables baseState, if (OdometryKalmanFilter.includeBias) { - OdometryTools.toSkewSymmetricMatrix(footMeasurements.positionMeasurement, skewMatrix); + OdometryTools.toSkewSymmetricMatrix(footSensing.positionMeasurement, skewMatrix); MatrixTools.setMatrixBlock(jacobianToPack, row, errorGyroBiasIndex, skewMatrix, 0, 0, 3, 3, -1.0); } } @@ -170,13 +169,7 @@ public void computeBaseMeasurementJacobian(StateVariables baseState, /** * This is equation 42 */ - public void computeFootMeasurementJacobian(StateVariables baseState, - StateVariables footState, - SensedVariables footMeasurements, - FrameVector3DReadOnly gravityVector, - int rowOffset, - int colOffset, - DMatrixRMaj jacobianToPack) + public void computeFootMeasurementJacobian(int rowOffset, int colOffset, DMatrixRMaj jacobianToPack) { OdometryTools.toRotationMatrix(baseState.orientation, rotationMatrixTranspose); CommonOps_DDRM.transpose(rotationMatrixTranspose); @@ -194,11 +187,11 @@ public void computeFootMeasurementJacobian(StateVariables baseState, // Second row. Partial of relative orientation error w.r.t. foot state row = rowOffset + measurementRelativeOrientationErrorIndex; - tempRotation.set(footMeasurements.orientationMeasurement); + tempRotation.set(footSensing.orientationMeasurement); tempRotation.multiplyConjugateOther(baseState.orientation); tempRotation.multiply(footState.orientation); - OdometryKalmanFilter.l3Operator(tempRotation, left); - CommonOps_DDRM.insert(left, jacobianToPack, row, colOffset + errorOrientationIndex); + 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; 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 index c05fbd310876..242eacd4181b 100644 --- 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 @@ -12,7 +12,6 @@ import us.ihmc.euclid.transform.interfaces.RigidBodyTransformReadOnly; import us.ihmc.euclid.tuple3D.Vector3D; import us.ihmc.euclid.tuple4D.Quaternion; -import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; import us.ihmc.matrixlib.MatrixTools; import us.ihmc.mecano.multiBodySystem.interfaces.FloatingJointBasics; import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; @@ -39,6 +38,7 @@ public class OdometryKalmanFilter extends ExtendedKalmanFilter { static final boolean includeBias = false; + static final boolean usePredictedStateInJacobian = true; // Constants and providers private static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); @@ -471,7 +471,7 @@ protected DMatrixRMaj linearizeProcessModel(DMatrixRMaj previousState) for (int i = 0; i < feetIMUs.size() + 1; i++) { - processModels.get(i).computeProcessJacobian(baseProcessState, estimatorDT, offset, AMatrix); + processModels.get(i).computeProcessJacobian(offset, AMatrix); offset += errorSizePerLink; } @@ -488,25 +488,12 @@ protected DMatrixRMaj linearizeMeasurementModel(DMatrixRMaj predictedState) for (int i = 0; i < feetIMUs.size(); i++) { MeasurementModel measurementModel = measurementModels.get(i); - measurementModel.computeBaseMeasurementJacobian(basePredictedState, feetPredictedState.get(i), feetSensing.get(i), rowOffset, CMatrix); - measurementModel.computeFootMeasurementJacobian(basePredictedState, feetPredictedState.get(i), feetSensing.get(i), gravityVector, rowOffset, colOffset, CMatrix); + measurementModel.computeBaseMeasurementJacobian(rowOffset, CMatrix); + measurementModel.computeFootMeasurementJacobian(rowOffset, colOffset, CMatrix); rowOffset += measurementSizePerLink; colOffset += errorSizePerLink; } return CMatrix; } - - static void l3Operator(QuaternionReadOnly quaternion, DMatrixRMaj matrixToPack) - { - OdometryTools.toSkewSymmetricMatrix(quaternion.getX(), quaternion.getY(), quaternion.getZ(), matrixToPack); - CommonOps_DDRM.addEquals(matrixToPack, quaternion.getS(), eye3x3); - } - - static void r3Operator(QuaternionReadOnly quaternion, DMatrixRMaj matrixToPack) - { - OdometryTools.toSkewSymmetricMatrix(quaternion.getX(), quaternion.getY(), quaternion.getZ(), matrixToPack); - CommonOps_DDRM.scale(-1.0, matrixToPack); - CommonOps_DDRM.addEquals(matrixToPack, quaternion.getS(), eye3x3); - } } 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 index fcdc140d3ea9..12c46880f1b7 100644 --- 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 @@ -1,15 +1,19 @@ 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; @@ -54,6 +58,17 @@ static void toSkewSymmetricMatrix(double x, double y, double z, DMatrixRMaj matr 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); @@ -140,4 +155,141 @@ public static void logMapSmallAngle(QuaternionReadOnly quaternion, Vector3DBasic 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 index 754597fd6aaf..b08887592814 100644 --- 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 @@ -6,6 +6,7 @@ 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; @@ -16,6 +17,8 @@ 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; @@ -77,8 +80,9 @@ public void update() /** * This is equation 39 */ - public void computeProcessJacobian(StateVariables stateVariables, double estimateDT, int offset, DMatrixRMaj jacobianToPack) + 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; @@ -90,18 +94,19 @@ public void computeProcessJacobian(StateVariables stateVariables, double estimat rowStart = offset + errorLinearVelocityIndex; CommonOps_DDRM.insert(eye3x3, jacobianToPack, rowStart, offset + errorLinearVelocityIndex); + OdometryTools.toRotationMatrix(stateVariables.orientation, rotationMatrix); OdometryTools.toSkewSymmetricMatrix(stateVariables.unbiasedAccel, skewMatrix); - MatrixTools.multAddBlock(-estimateDT, rotationMatrix, skewMatrix, jacobianToPack, rowStart, offset + errorOrientationIndex); + MatrixTools.multAddBlock(-estimatorDt, rotationMatrix, skewMatrix, jacobianToPack, rowStart, offset + errorOrientationIndex); if (OdometryKalmanFilter.includeBias) - MatrixTools.setMatrixBlock(jacobianToPack, rowStart, offset + errorAccelBiasIndex, rotationMatrix, 0, 0, 3, 3, -estimateDT); + 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, -estimateDT); + 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); 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 index 1980cc940f47..12e24dc5bd9e 100644 --- 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 @@ -47,7 +47,7 @@ public void initialize() gyroBias.setToZero(); } - public void set(int start, DMatrixRMaj state, SensedVariables measuredVariables) + public void set(int start, DMatrixRMaj state, SensedVariables sensedVariables) { translation.set(start + stateTranslationIndex, state); linearVelocity.set(start + stateLinearVelocityIndex, state); @@ -57,13 +57,13 @@ public void set(int start, DMatrixRMaj state, SensedVariables measuredVariables) if (OdometryKalmanFilter.includeBias) { - unbiasedAccel.sub(measuredVariables.accelMeasurement, accelBias); - unbiasedGyro.sub(measuredVariables.gyroMeasurement, gyroBias); + unbiasedAccel.sub(sensedVariables.accelMeasurement, accelBias); + unbiasedGyro.sub(sensedVariables.gyroMeasurement, gyroBias); } else { - unbiasedAccel.set(measuredVariables.accelMeasurement); - unbiasedGyro.set(measuredVariables.gyroMeasurement); + unbiasedAccel.set(sensedVariables.accelMeasurement); + unbiasedGyro.set(sensedVariables.gyroMeasurement); } } 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..3107796d2959 --- /dev/null +++ b/ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/MeasurementModelTest.java @@ -0,0 +1,325 @@ +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.Quaternion; +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.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() + { + fail("Need to write this."); + } + @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 processModel = 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(); + + 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); + + processModel.computeBaseMeasurementJacobian( 0, jacobian); + processModel.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, 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(footState.translation, 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), 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()), toVector(b), vector); + + return vector; + } + + 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; + } + + + + 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..7c4dbf2ead66 --- /dev/null +++ b/ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdomTestTools.java @@ -0,0 +1,48 @@ +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 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; + } +} 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 index 76d58f545c32..741ec03ed7df 100644 --- 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 @@ -10,6 +10,7 @@ import us.ihmc.euclid.tools.EuclidCoreTestTools; import us.ihmc.euclid.tuple3D.Vector3D; import us.ihmc.euclid.tuple4D.Quaternion; +import us.ihmc.matrixlib.MatrixTools; import java.util.Random; 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..229b7cab346f --- /dev/null +++ b/ihmc-state-estimation/src/test/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/ProcessModelTest.java @@ -0,0 +1,347 @@ +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); + predictedState.orientation.get(predictedRotation); + 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) + { + + } + } +} From f7bd954ba71f16bec595f2268dc22e1ff0c85535 Mon Sep 17 00:00:00 2001 From: Robert Date: Tue, 2 Sep 2025 22:54:35 -0500 Subject: [PATCH 09/18] fixed a couple of the last little stupid bugs --- .../ExtendedKalmanFilter.java | 1 + .../odomEKF/MeasurementModel.java | 5 +- .../odomEKF/OdometryKalmanFilter.java | 10 +- .../odomEKF/OdometryTools.java | 17 -- .../odomEKF/SensedVariables.java | 5 - .../odomEKF/MeasurementModelTest.java | 164 +++++++++++++++--- .../odomEKF/OdomTestTools.java | 19 ++ .../odomEKF/OdometryToolsTest.java | 26 +++ .../odomEKF/ProcessModelTest.java | 3 +- 9 files changed, 192 insertions(+), 58 deletions(-) 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 4b65b5f4cad3..2b460f0a9e3a 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.log.LogTools; /** * 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 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 index 45473b51ac43..183aaae88e16 100644 --- 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 @@ -41,7 +41,6 @@ class MeasurementModel 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 DMatrixRMaj lr3x3 = new DMatrixRMaj(3, 3); private final Quaternion tempRotation = new Quaternion(); public MeasurementModel(String prefix, @@ -73,7 +72,7 @@ public void update(DMatrixRMaj 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.transform(predictedMeasurements.relativePosition); // FIXME is this right? transforms do more operations than I think is expected + 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); @@ -98,7 +97,7 @@ public void update(DMatrixRMaj footCovariance) predictedMeasurements.contactVelocity.set(footState.linearVelocity); // Fifth row. Set the predicted gravity vector - footState.orientation.transform(gravityVector, predictedMeasurements.accelMeasure); + footState.orientation.inverseTransform(gravityVector, predictedMeasurements.accelMeasure); if (OdometryKalmanFilter.includeBias) predictedMeasurements.accelMeasure.add(footState.accelBias); } 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 index 242eacd4181b..85bd3c291c1a 100644 --- 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 @@ -37,7 +37,7 @@ */ public class OdometryKalmanFilter extends ExtendedKalmanFilter { - static final boolean includeBias = false; + static final boolean includeBias = true; static final boolean usePredictedStateInJacobian = true; // Constants and providers @@ -134,10 +134,10 @@ public OdometryKalmanFilter(IMUSensorReadOnly baseIMU, orientationCovariance.set(1e-2); biasCovariance.set(1e-4); - measuredTranslationCovariance.set(1e-5); - measuredOrientationCovariance.set(1e-2); - velocityErrorCovariance.set(1e-3); - contactVelocityCovariance.set(1e-5); + measuredTranslationCovariance.set(1e-8); + measuredOrientationCovariance.set(1e-8); + velocityErrorCovariance.set(1e-5); + contactVelocityCovariance.set(1e-6); contactAccelerationCovariance.set(1.0); // translationCovariance.set(1e-4); 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 index 12c46880f1b7..93c8f3fa3f14 100644 --- 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 @@ -113,23 +113,6 @@ static void setDiagonals(int start, int elements, double value, DMatrixRMaj matr matrix.set(i, i, value); } - public static void toQuaternionFromRotationVector(Vector3DReadOnly rotation, QuaternionBasics quaternionToPack) - { - double magnitude = rotation.norm(); - if (magnitude < 1e-7) - { - quaternionToPack.setToZero(); - return; - } - - double s = Math.sin(magnitude * 0.5); - double qs = Math.cos(magnitude * 0.5); - 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 toQuaternionFromRotationVectorSmallAngle(Vector3DReadOnly rotation, QuaternionBasics quaternionToPack) { double magnitude = rotation.norm(); 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 index 7d38a65007e5..a94c0c60a478 100644 --- 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 @@ -79,11 +79,6 @@ public void update() positionMeasurement.setFromReferenceFrame(imu.getMeasurementFrame()); orientationMeasurement.setFromReferenceFrame(imu.getMeasurementFrame()); - FramePoint3D imuInWorld = new FramePoint3D(imu.getMeasurementFrame()); - FramePoint3D baseInWorld = new FramePoint3D(baseIMU.getMeasurementFrame()); - imuInWorld.changeFrame(worldFrame); - baseInWorld.changeFrame(worldFrame); - imu.getMeasurementFrame().getTwistRelativeToOther(baseIMU.getMeasurementFrame(), twist); twist.changeFrame(worldFrame); linearVelocity.set(twist.getLinearPart()); 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 index 3107796d2959..49bf96e8b6fc 100644 --- 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 @@ -12,7 +12,6 @@ import us.ihmc.euclid.transform.RigidBodyTransform; 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 us.ihmc.mecano.frames.MovingReferenceFrame; @@ -20,6 +19,8 @@ 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; @@ -35,8 +36,133 @@ public class MeasurementModelTest @Test public void testUpdate() { - fail("Need to write this."); + 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() { @@ -53,13 +179,15 @@ public void testComputeMeasurementJacobian() SensedVariables footSensing = new SensedVariables("footSensed", baseIMU, footIMU, new TestBiasProvider(), test); MeasurementVariables measurement = new MeasurementVariables("footMeasure", test); - MeasurementModel processModel = new MeasurementModel("foot0", baseState, footState, footSensing, measurement, gravity, () -> Double.MAX_VALUE, 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); @@ -77,8 +205,8 @@ public void testComputeMeasurementJacobian() OdomTestTools.setRandomState(random, baseSensing, baseState); OdomTestTools.setRandomState(random, footSensing, footState); - processModel.computeBaseMeasurementJacobian( 0, jacobian); - processModel.computeFootMeasurementJacobian( 0, OdometryIndexHelper.errorSizePerLink, jacobian); + 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); @@ -115,7 +243,7 @@ public void testComputeMeasurementJacobian() MatrixTools.setMatrixBlock(jacobianExpected, 3, 6, block, 1, 1, 3, 3, -1.0); // Row 2, foot - block = OdometryTools.lOperator(multiply(footSensing.orientationMeasurement, fromVector(multiplyLeftInverse(baseState.orientation, footState.orientation)))); + 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 @@ -132,7 +260,7 @@ public void testComputeMeasurementJacobian() CommonOps_DDRM.insert(skewMatrix, jacobianExpected, 6, 6); if (OdometryKalmanFilter.includeBias) { - OdometryTools.toSkewSymmetricMatrix(footState.translation, skewMatrix); + OdometryTools.toSkewSymmetricMatrix(footSensing.positionMeasurement, skewMatrix); MatrixTools.setMatrixBlock(jacobianExpected, 6, 12, skewMatrix, 0, 0, 3, 3, -1.0); } @@ -167,7 +295,7 @@ public void testComputeMeasurementJacobian() static DMatrixRMaj multiply(QuaternionReadOnly a, QuaternionReadOnly b) { DMatrixRMaj vector = new DMatrixRMaj(4, 1); - CommonOps_DDRM.mult(OdometryTools.lOperator(a), toVector(b), vector); + CommonOps_DDRM.mult(OdometryTools.lOperator(a), OdomTestTools.toVector(b), vector); return vector; } @@ -175,29 +303,11 @@ static DMatrixRMaj multiply(QuaternionReadOnly a, QuaternionReadOnly b) 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()), toVector(b), vector); + CommonOps_DDRM.mult(OdometryTools.lOperator(a.getS(), -a.getX(), -a.getY(), -a.getZ()), OdomTestTools.toVector(b), vector); return vector; } - 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; - } - - - static class TestBiasProvider implements IMUBiasProvider { 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 index 7c4dbf2ead66..41e7dadfa740 100644 --- 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 @@ -3,6 +3,8 @@ 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; @@ -45,4 +47,21 @@ public static DMatrixRMaj setRandomState(Random random, SensedVariables sensedVa 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 index 741ec03ed7df..ae6d107be4ee 100644 --- 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 @@ -9,7 +9,9 @@ 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; @@ -122,4 +124,28 @@ public void testToRotationMatrixInverse() 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 index 229b7cab346f..2e38e7920f91 100644 --- 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 @@ -125,7 +125,7 @@ public void testUpdate() predictedState.translation.get(predictedTranslation); predictedState.linearVelocity.get(predictedLinearVelocity); - predictedState.orientation.get(predictedRotation); + predictedRotation.set(OdomTestTools.toVector(predictedState.orientation)); predictedState.accelBias.get(predictedAccelBias); predictedState.gyroBias.get(predictedGyroBias); @@ -136,6 +136,7 @@ public void testUpdate() EjmlUnitTests.assertEquals(expectedPredictedGyroBias, predictedGyroBias, 1e-6); } } + @Test public void testComputeProcessJacobian() { From 58ddc5c569bc978cb36eb740d078ad2352b9fd22 Mon Sep 17 00:00:00 2001 From: Robert Date: Tue, 2 Sep 2025 23:16:39 -0500 Subject: [PATCH 10/18] fixed some more variables --- .../odomEKF/MeasurementModel.java | 11 +++++++-- .../odomEKF/OdometryKalmanFilter.java | 24 ++++--------------- 2 files changed, 14 insertions(+), 21 deletions(-) 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 index 183aaae88e16..dd2b4c99f348 100644 --- 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 @@ -11,6 +11,7 @@ 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.*; @@ -28,6 +29,7 @@ class MeasurementModel public final YoQuaternion footOrientationInBaseFrame; public final YoBoolean isInContact; + public final YoDouble footVariance; private final DoubleProvider contactThreshold; // Temp variables @@ -61,6 +63,7 @@ public MeasurementModel(String prefix, footOrientationInBaseFrame = new YoQuaternion(prefix + "OrientationInBaseFrame", registry); isInContact = new YoBoolean(prefix + "IsInContact", registry); + footVariance = new YoDouble(prefix + "FootVariance", registry); } /** @@ -91,7 +94,9 @@ public void update(DMatrixRMaj footCovariance) predictedMeasurements.relativeLinearVelocityError.add(footSensing.linearVelocity); if (!isInContact.getBooleanValue()) + { return; + } // Fourth row. Set the predicted linear velocity predictedMeasurements.contactVelocity.set(footState.linearVelocity); @@ -108,13 +113,15 @@ public void update(DMatrixRMaj footCovariance) private boolean computeIsFootInContact(DMatrixRMaj footCovariance) { - footState.translation.get(vector); + 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 tempScalar.get(0, 0) < contactThreshold.getValue(); + return footVariance.getValue() < contactThreshold.getValue(); } public boolean getIsFootInContact() 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 index 85bd3c291c1a..9ebc99aeea5f 100644 --- 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 @@ -49,7 +49,6 @@ public class OdometryKalmanFilter extends ExtendedKalmanFilter private final List feetIMUs; private final IMUBiasProvider imuBiasProvider; - private final double estimatorDT; private final FrameVector3DReadOnly gravityVector; private final YoDouble translationCovariance = new YoDouble("translationCovariance", registry); @@ -122,41 +121,28 @@ public OdometryKalmanFilter(IMUSensorReadOnly baseIMU, { super(stateSizePerLink * (1 + feetIMUs.size()), errorSizePerLink * (1 + feetIMUs.size()) , measurementSizePerLink * feetIMUs.size()); - int links = 1 + feetIMUs.size(); // Initialize covariance processCovariance = new DMatrixRMaj(getErrorSize(), getErrorSize()); measurementCovariance = new DMatrixRMaj(getMeasurementSize(), getMeasurementSize()); MatrixTools.setDiagonal(processCovariance, 1e-2); MatrixTools.setDiagonal(measurementCovariance, 1e-3); - translationCovariance.set(1e-3); + translationCovariance.set(1e-5); velocityCovariance.set(1e-2); orientationCovariance.set(1e-2); biasCovariance.set(1e-4); - measuredTranslationCovariance.set(1e-8); - measuredOrientationCovariance.set(1e-8); + measuredTranslationCovariance.set(1e-6); + measuredOrientationCovariance.set(1e-5); velocityErrorCovariance.set(1e-5); - contactVelocityCovariance.set(1e-6); + contactVelocityCovariance.set(1e-10); contactAccelerationCovariance.set(1.0); -// translationCovariance.set(1e-4); -// velocityCovariance.set(1e-4); -// orientationCovariance.set(1e-4); -// biasCovariance.set(1e-4); -// -// measuredTranslationCovariance.set(1e-4); -// measuredOrientationCovariance.set(1e-4); -// velocityErrorCovariance.set(1e-4); -// contactVelocityCovariance.set(1e-4); -// contactAccelerationCovariance.set(1e-4); - contactThreshold.set(Double.MAX_VALUE); this.baseIMU = baseIMU; this.feetIMUs = feetIMUs; this.imuBiasProvider = baseImuBiasProvider; - this.estimatorDT = estimatorDT; predictedState = new DMatrixRMaj(getStateSize(), 1); predictedMeasurement = new DMatrixRMaj(getMeasurementSize(), 1); @@ -451,7 +437,7 @@ public DMatrixRMaj measurementModel(DMatrixRMaj predictedState) // 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) + errorLinearVelocityIndex; + int row = errorSizePerLink * (i + 1) + measurementContactVelocityIndex; MatrixTools.setMatrixBlock(footCovariance, 0, 0, getCovariance(), row, row, 3, 3, 1.0); measurementModels.get(i).update(footCovariance); From 32f2c0900b84f90e2f095588244057ff9864ef9b Mon Sep 17 00:00:00 2001 From: Robert Date: Wed, 3 Sep 2025 17:28:44 -0500 Subject: [PATCH 11/18] started trying to implement the filter weights from the internet --- .../ExtendedKalmanFilter.java | 78 ++++++++++------- .../odomEKF/MeasurementModel.java | 15 +--- .../odomEKF/MeasurementVariables.java | 8 +- .../odomEKF/OdometryKalmanFilter.java | 83 +++++++++++-------- .../OdometryKalmanFilterParameters.java | 53 ++++++++++++ .../odomEKF/SensedVariables.java | 5 +- 6 files changed, 159 insertions(+), 83 deletions(-) create mode 100644 ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryKalmanFilterParameters.java 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 2b460f0a9e3a..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,7 +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.log.LogTools; +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 @@ -91,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. */ @@ -143,9 +159,6 @@ 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; @@ -165,9 +178,11 @@ public ExtendedKalmanFilter(int stateSize, int errorSize, int measurementSize) processCovariance = new DMatrixRMaj(errorSize, errorSize); measurementCovariance = new DMatrixRMaj(measurementSize, measurementSize); + maskedMeasurementCovariance = new DMatrixRMaj(measurementSize, measurementSize); 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); @@ -176,8 +191,10 @@ public ExtendedKalmanFilter(int stateSize, int errorSize, int measurementSize) predictedCovarianceContainer = new DMatrixRMaj(errorSize, errorSize); measurementResidual = new DMatrixRMaj(measurementSize, 1); + 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); @@ -191,9 +208,6 @@ public ExtendedKalmanFilter(int stateSize, int errorSize, int measurementSize) normalizedInnovationContainer = new DMatrixRMaj(measurementSize, 1); josephTransposeTerm = new DMatrixRMaj(errorSize, errorSize); - josephTransposeTermContainer = new DMatrixRMaj(errorSize, measurementSize); - josephIdentity = CommonOps_DDRM.identity(errorSize); - josephGainTerm = new DMatrixRMaj(errorSize, errorSize); josephGainTermContainer = new DMatrixRMaj(errorSize, errorSize); } @@ -352,7 +366,8 @@ protected void updateStep() calculateKalmanGain(); // x_(k|k) = x_(k|k-1) + K_k * y_k - CommonOps_DDRM.mult(kalmanGain, measurementResidual, stateCorrection); + 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(); @@ -363,22 +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); + maskResidualCovarianceForOutlierRejection(); + + solver.setA(maskedResidualCovariance); // fixme this doesn't work. - CommonOps_DDRM.invert(residualCovariance, inverseResidualCovariance); + 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); } @@ -389,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); } } @@ -426,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]; } 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 index dd2b4c99f348..bbbbdb11f499 100644 --- 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 @@ -28,7 +28,6 @@ class MeasurementModel // State variables public final YoQuaternion footOrientationInBaseFrame; - public final YoBoolean isInContact; public final YoDouble footVariance; private final DoubleProvider contactThreshold; @@ -62,7 +61,6 @@ public MeasurementModel(String prefix, this.contactThreshold = contactThreshold; footOrientationInBaseFrame = new YoQuaternion(prefix + "OrientationInBaseFrame", registry); - isInContact = new YoBoolean(prefix + "IsInContact", registry); footVariance = new YoDouble(prefix + "FootVariance", registry); } @@ -71,7 +69,7 @@ public MeasurementModel(String prefix, */ public void update(DMatrixRMaj footCovariance) { - isInContact.set(computeIsFootInContact(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); @@ -93,11 +91,6 @@ public void update(DMatrixRMaj footCovariance) predictedMeasurements.relativeLinearVelocityError.sub(velocityError); predictedMeasurements.relativeLinearVelocityError.add(footSensing.linearVelocity); - if (!isInContact.getBooleanValue()) - { - return; - } - // Fourth row. Set the predicted linear velocity predictedMeasurements.contactVelocity.set(footState.linearVelocity); @@ -126,7 +119,7 @@ private boolean computeIsFootInContact(DMatrixRMaj footCovariance) public boolean getIsFootInContact() { - return isInContact.getBooleanValue(); + return footSensing.isInContact.getBooleanValue(); } /** @@ -137,7 +130,6 @@ public void computeBaseMeasurementJacobian(int rowOffset, DMatrixRMaj jacobianTo 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); @@ -203,9 +195,6 @@ public void computeFootMeasurementJacobian(int rowOffset, int colOffset, DMatrix row = rowOffset + measurementRelativeVelocityIndex; CommonOps_DDRM.insert(rotationMatrixTranspose, jacobianToPack, row, colOffset + errorLinearVelocityIndex); - if (!isInContact.getBooleanValue()) - return; - // 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; 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 index 8c579577c615..d01c4de05a50 100644 --- 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 @@ -3,6 +3,7 @@ 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; @@ -16,6 +17,7 @@ class MeasurementVariables public final YoVector3D contactVelocity; public final YoVector3D accelMeasure; + public MeasurementVariables(String prefix, YoRegistry registry) { relativePosition = new YoVector3D(prefix + "RelativePosition", registry); @@ -25,15 +27,11 @@ public MeasurementVariables(String prefix, YoRegistry registry) accelMeasure = new YoVector3D(prefix + "AccelMeasure", registry); } - public void get(boolean isInContact, int start, DMatrixRMaj measurementToPack) + public void get(int start, DMatrixRMaj measurementToPack) { relativePosition.get(start + measurementRelativeTranslationIndex, measurementToPack); relativeOrientationError.get(start + measurementRelativeOrientationErrorIndex, measurementToPack); relativeLinearVelocityError.get(start + measurementRelativeVelocityIndex, measurementToPack); - - if (!isInContact) - return; - 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/OdometryKalmanFilter.java b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/OdometryKalmanFilter.java index 9ebc99aeea5f..7f6c91825574 100644 --- 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 @@ -51,16 +51,7 @@ public class OdometryKalmanFilter extends ExtendedKalmanFilter private final FrameVector3DReadOnly gravityVector; - private final YoDouble translationCovariance = new YoDouble("translationCovariance", registry); - private final YoDouble velocityCovariance = new YoDouble("velocityCovariance", registry); - private final YoDouble orientationCovariance = new YoDouble("orientationCovariance", registry); - private final YoDouble biasCovariance = new YoDouble("biasCovariance", registry); - - private final YoDouble measuredTranslationCovariance = new YoDouble("measuredTranslationCovariance", registry); - private final YoDouble measuredOrientationCovariance = new YoDouble("measuredOrientationCovariance", registry); - private final YoDouble velocityErrorCovariance = new YoDouble("velocityErrorCovariance", registry); - private final YoDouble contactVelocityCovariance = new YoDouble("contactVelocityCovariance", registry); - private final YoDouble contactAccelerationCovariance = new YoDouble("contactAccelerationMeasureCovariance", registry); + private final OdometryKalmanFilterParameters parameters = new OdometryKalmanFilterParameters(registry); private final YoDouble contactThreshold = new YoDouble("contactVarianceThreshold", registry); @@ -127,18 +118,7 @@ public OdometryKalmanFilter(IMUSensorReadOnly baseIMU, MatrixTools.setDiagonal(processCovariance, 1e-2); MatrixTools.setDiagonal(measurementCovariance, 1e-3); - translationCovariance.set(1e-5); - velocityCovariance.set(1e-2); - orientationCovariance.set(1e-2); - biasCovariance.set(1e-4); - - measuredTranslationCovariance.set(1e-6); - measuredOrientationCovariance.set(1e-5); - velocityErrorCovariance.set(1e-5); - contactVelocityCovariance.set(1e-10); - contactAccelerationCovariance.set(1.0); - - contactThreshold.set(Double.MAX_VALUE); + contactThreshold.set(0.5); this.baseIMU = baseIMU; this.feetIMUs = feetIMUs; @@ -265,24 +245,31 @@ public void compute() private void updateCovariance() { - for (int i = 0; i < feetIMUs.size() + 1; i++) + 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 * errorSizePerLink; - OdometryTools.setDiagonals(index + errorTranslationIndex, 3, translationCovariance.getValue(), processCovariance); - OdometryTools.setDiagonals(index + errorLinearVelocityIndex, 3, velocityCovariance.getValue(), processCovariance); - OdometryTools.setDiagonals(index + errorOrientationIndex, 3, orientationCovariance.getValue(), processCovariance); - OdometryTools.setDiagonals(index + errorAccelBiasIndex, 3, biasCovariance.getValue(), processCovariance); - OdometryTools.setDiagonals(index + errorGyroBiasIndex, 3, biasCovariance.getValue(), processCovariance); + 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, measuredTranslationCovariance.getValue(), measurementCovariance); - OdometryTools.setDiagonals(index + measurementRelativeOrientationErrorIndex, 3, measuredOrientationCovariance.getValue(), measurementCovariance); - OdometryTools.setDiagonals(index + measurementRelativeVelocityIndex, 3, velocityErrorCovariance.getValue(), measurementCovariance); - OdometryTools.setDiagonals(index + measurementContactVelocityIndex, 3, contactVelocityCovariance.getValue(), measurementCovariance); - OdometryTools.setDiagonals(index + measurementAccelIndex, 3, contactAccelerationCovariance.getValue(), measurementCovariance); + 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); @@ -441,7 +428,7 @@ public DMatrixRMaj measurementModel(DMatrixRMaj predictedState) MatrixTools.setMatrixBlock(footCovariance, 0, 0, getCovariance(), row, row, 3, 3, 1.0); measurementModels.get(i).update(footCovariance); - feetMeasurements.get(i).get(measurementModels.get(i).getIsFootInContact(), i * measurementSizePerLink, predictedMeasurement); + feetMeasurements.get(i).get(i * measurementSizePerLink, predictedMeasurement); } // return the predicted measurement @@ -482,4 +469,30 @@ protected DMatrixRMaj linearizeMeasurementModel(DMatrixRMaj predictedState) 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/SensedVariables.java b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/odomEKF/SensedVariables.java index a94c0c60a478..ce08c1996dae 100644 --- 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 @@ -1,6 +1,5 @@ package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation.odomEKF; -import us.ihmc.euclid.referenceFrame.FramePoint3D; import us.ihmc.euclid.referenceFrame.FrameVector3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; @@ -12,6 +11,7 @@ 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 { @@ -32,6 +32,8 @@ class SensedVariables 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(); @@ -52,6 +54,7 @@ public SensedVariables(String prefix, IMUSensorReadOnly baseIMU, IMUSensorReadOn orientationMeasurement = new YoFrameQuaternion(prefix + "OrientationMeasurement", baseIMU.getMeasurementFrame(), registry); linearVelocity = new YoFrameVector3D(prefix + "LinearVelocity", worldFrame, registry); + isInContact = new YoBoolean(prefix + "IsInContact", registry); } public void update() From 87a797019b5ee9fa03af2cc6d78dcf9b0bd9009e Mon Sep 17 00:00:00 2001 From: Robert Date: Wed, 3 Sep 2025 22:17:58 -0500 Subject: [PATCH 12/18] extracted the foot estimator --- ...sKinematicsBasedLinearStateCalculator.java | 293 +--------------- .../SingleFootEstimator.java | 325 ++++++++++++++++++ 2 files changed, 329 insertions(+), 289 deletions(-) create mode 100644 ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/SingleFootEstimator.java 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..28b22dd874c6 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,43 +4,23 @@ 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.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.AlphaFilteredYoVariable; @@ -245,13 +225,11 @@ public void estimatePelvisLinearState(List trustedFeet, List 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); - } - - /** - * updates the position of a swinging foot - * - * @param swingingFoot a foot in swing - * @param pelvisPosition the current pelvis position - */ - private void updateUntrustedFootPosition(FramePoint3DReadOnly pelvisPosition) - { - footPositionInWorld.sub(pelvisPosition, footToRootJointPosition); - - copPositionInWorld.set(footPositionInWorld); - - copRawInFootFrame.setToZero(); - copFilteredInFootFrame.setToZero(); - } - - private final FrameVector3D tempFrameVector = new FrameVector3D(); - - private void updateTrustedFootPosition(boolean trustCoPAsNonSlippingContactPoint, FramePoint3DReadOnly rootJointPosition) - { - if (trustCoPAsNonSlippingContactPoint) - { - 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 - } - - footPositionInWorld.set(rootJointPosition); - footPositionInWorld.sub(footToRootJointPosition); - } - - private final FramePoint2D tempCoP2d = new FramePoint2D(); - private final FrameVector3D tempCoPOffset = new FrameVector3D(); - - /** - * 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); - } - - 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); - - footTwistInWorld.getLinearVelocityAt(copPositionInWorld, footLinearVelocityToPack); - } + return footEstimatorMap.get(foot).getFootVelocityInWorld(); } @Override 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..29d6dc63497f --- /dev/null +++ b/ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/SingleFootEstimator.java @@ -0,0 +1,325 @@ +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.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.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.YoGraphicPoint2DDefinition; +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.AlphaFilteredYoVariable; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; + +class SingleFootEstimator implements SCS2YoGraphicHolder +{ + 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 + */ + public 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); + } + + /** + * updates the position of a swinging foot + * + * @param swingingFoot a foot in swing + * @param pelvisPosition the current pelvis position + */ + public void updateUntrustedFootPosition(FramePoint3DReadOnly pelvisPosition) + { + footPositionInWorld.sub(pelvisPosition, footToRootJointPosition); + + copPositionInWorld.set(footPositionInWorld); + + copRawInFootFrame.setToZero(); + copFilteredInFootFrame.setToZero(); + } + + private final FrameVector3D tempFrameVector = new FrameVector3D(); + + public void updateTrustedFootPosition(boolean trustCoPAsNonSlippingContactPoint, FramePoint3DReadOnly rootJointPosition) + { + if (trustCoPAsNonSlippingContactPoint) + { + 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 + } + + footPositionInWorld.set(rootJointPosition); + footPositionInWorld.sub(footToRootJointPosition); + } + + public void setCoPZPositionInWorld(boolean useControllerDesiredCoP, double zPosition) + { + updateCoPPosition(true, useControllerDesiredCoP); + copPositionInWorld.setZ(zPosition); + correctFootPositionsUsingCoP(true); + } + + public void setFootZPositionInWorld(double zPosition) + { + footPositionInWorld.setZ(zPosition); + } + + private final FramePoint2D tempCoP2d = new FramePoint2D(); + private final FrameVector3D tempCoPOffset = new FrameVector3D(); + + /** + * Compute the foot CoP. The CoP is the point on the support foot trusted to be not slipping. + * + * @param trustedSide + * @param footSwitch + */ + public 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 + */ + public 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(); + + public void updateFootLinearVelocityInWorld(TwistReadOnly rootBodyTwist) + { + computeFootLinearVelocityInWorld(rootBodyTwist, footVelocityInWorld); + } + + 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); + + footTwistInWorld.getLinearVelocityAt(copPositionInWorld, footLinearVelocityToPack); + } + + public FrameVector3DReadOnly getFootToRootJointPosition() + { + return footToRootJointPosition; + } + + public FrameVector3DReadOnly getFootVelocityInWorld() + { + return footVelocityInWorld; + } +} From 0a7f98c5e38c1d86b5588f685af92a3874983259 Mon Sep 17 00:00:00 2001 From: Robert Date: Wed, 3 Sep 2025 23:53:55 -0500 Subject: [PATCH 13/18] added the imu as a filter for the feet --- ...sKinematicsBasedLinearStateCalculator.java | 75 +++-- .../PelvisLinearStateUpdater.java | 12 + .../SingleFootEstimator.java | 313 +++++++++++++----- 3 files changed, 278 insertions(+), 122 deletions(-) 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 28b22dd874c6..845db3663e16 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 @@ -5,9 +5,12 @@ import java.util.Map; import us.ihmc.euclid.referenceFrame.FramePoint3D; +import us.ihmc.euclid.referenceFrame.FrameVector3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly; import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; +import us.ihmc.euclid.referenceFrame.tools.EuclidFrameFactories; +import us.ihmc.euclid.tuple3D.Vector3D; import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry; import us.ihmc.humanoidRobotics.model.CenterOfPressureDataHolder; import us.ihmc.mecano.multiBodySystem.interfaces.FloatingJointBasics; @@ -15,6 +18,7 @@ import us.ihmc.mecano.spatial.Twist; import us.ihmc.robotics.SCS2YoGraphicHolder; import us.ihmc.robotics.contactable.ContactablePlaneBody; +import us.ihmc.sensorProcessing.stateEstimation.IMUSensorReadOnly; import us.ihmc.yoVariables.euclid.filters.BacklashCompensatingVelocityYoFrameVector3D; import us.ihmc.robotics.sensors.FootSwitchInterface; import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinition; @@ -76,8 +80,12 @@ public class PelvisKinematicsBasedLinearStateCalculator implements SCS2YoGraphic public PelvisKinematicsBasedLinearStateCalculator(FullInverseDynamicsStructure inverseDynamicsStructure, Map feetContactablePlaneBodies, Map footSwitches, + Map footIMUs, + IMUBiasProvider imuBiasProvider, CenterOfPressureDataHolder centerOfPressureDataHolderFromController, + BooleanProvider cancelGravityFromAccelerationMeasurement, double estimatorDT, + double gravitationalAcceleration, StateEstimatorParameters stateEstimatorParameters, YoGraphicsListRegistry yoGraphicsListRegistry, YoRegistry parentRegistry) @@ -98,6 +106,16 @@ public PelvisKinematicsBasedLinearStateCalculator(FullInverseDynamicsStructure i copFilterBreakFrequency = new DoubleParameter("CopFilterBreakFrequency", registry, stateEstimatorParameters.getCoPFilterFreqInHertz()); correctTrustedFeetPositions = new BooleanParameter("correctTrustedFeetPositions", registry, stateEstimatorParameters.correctTrustedFeetPositions()); + YoDouble footAlphaLeakIMUOnly = new YoDouble("footIMUOnlyAlphaLeak", registry); + YoDouble imuAgainstKinematicsForVelocityBreakFrequency = new YoDouble("footIMUAgainstKinematicsForVelocityBreakFrequency", registry); + YoDouble imuAgainstKinematicsForPositionBreakFrequency = new YoDouble("footIMUAgainstKinematicsForPositionBreakFrequency", registry); + imuAgainstKinematicsForPositionBreakFrequency.set(200.0); + imuAgainstKinematicsForVelocityBreakFrequency.set(2.0); + footAlphaLeakIMUOnly.set(0.999); + + FrameVector3DReadOnly gravityVector = EuclidFrameFactories.newLinkedFrameVector3DReadOnly(() -> worldFrame, new Vector3D(0, 0, -Math.abs(gravitationalAcceleration))); + + footEstimators = new SingleFootEstimator[feetRigidBodies.length]; for (int i = 0; i < feetRigidBodies.length; i++) { @@ -107,9 +125,16 @@ public PelvisKinematicsBasedLinearStateCalculator(FullInverseDynamicsStructure i footEstimators[i] = new SingleFootEstimator(rootJoint, contactableFoot, footSwitch, + footIMUs.get(footRigidBody), + imuBiasProvider, footToRootJointPositionBreakFrequency, copFilterBreakFrequency, centerOfPressureDataHolderFromController, + cancelGravityFromAccelerationMeasurement, + gravityVector, + imuAgainstKinematicsForPositionBreakFrequency, + imuAgainstKinematicsForVelocityBreakFrequency, + footAlphaLeakIMUOnly, estimatorDT, registry); footEstimatorMap.put(footRigidBody, footEstimators[i]); @@ -160,36 +185,32 @@ public void initialize(FramePoint3DReadOnly pelvisPosition) for (SingleFootEstimator footEstimator : footEstimators) { - footEstimator.updateUntrustedFootPosition(pelvisPosition); + footEstimator.correctFootPositionFromRootJoint(false, false, pelvisPosition); } kinematicsIsUpToDate.set(false); } + private final Twist tempRootBodyTwist = new Twist(); + /** * 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); + { + footEstimator.updateKinematics(tempRootBodyTwist); + } + + kinematicsIsUpToDate.set(true); } + /** * Updates this module when no foot can be trusted. * @@ -202,7 +223,7 @@ public void updateNoTrustedFeet(FramePoint3DReadOnly pelvisPosition, FrameVector { for (SingleFootEstimator footEstimator : footEstimators) { - footEstimator.updateUntrustedFootPosition(pelvisPosition); + footEstimator.correctFootPositionFromRootJoint(false, false, pelvisPosition); } rootJointPosition.set(pelvisPosition); @@ -218,31 +239,25 @@ public void estimatePelvisLinearState(List trustedFeet, List trustedFeet, List footIMUs = new HashMap<>(); + for (IMUSensorReadOnly imu : imuProcessedOutputs) + { + if (feetContactablePlaneBodies.containsKey(imu.getMeasurementLink())) + footIMUs.put(imu.getMeasurementLink(), imu); + } + kinematicsBasedLinearStateCalculator = new PelvisKinematicsBasedLinearStateCalculator(inverseDynamicsStructure, feetContactablePlaneBodies, footSwitches, + footIMUs, + imuBiasProvider, centerOfPressureDataHolderFromController, + cancelGravityFromAccelerationMeasurement, estimatorDT, + gravitationalAcceleration, stateEstimatorParameters, yoGraphicsListRegistry, registry); 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 index 29d6dc63497f..8b6c0b449991 100644 --- 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 @@ -29,70 +29,113 @@ 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.YoGraphicPoint2DDefinition; +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.AlphaFilteredYoVariable; +import us.ihmc.yoVariables.filters.AlphaFilterTools; +import us.ihmc.yoVariables.providers.BooleanProvider; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoRegistry; class SingleFootEstimator implements SCS2YoGraphicHolder { + private static boolean USE_IMU_DATA = true; + private final RigidBodyBasics foot; private final ReferenceFrame rootJointFrame; private final ReferenceFrame soleFrame; private final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); - private final YoFrameVector3D footVelocityInWorld; + 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 YoFrameVector3D copVelocityInWorld; private final AlphaFilteredYoFrameVector3D footToRootJointPosition; private final YoFramePoint3D footPositionInWorld; /** Debug variable */ private final YoFramePoint3D rootJointPositionPerFoot; + private final YoFramePoint3D copPositionInWorld; + 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 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, + FrameVector3DReadOnly gravityVector, + 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.gravityVector = gravityVector; + this.imuAgainstKinematicsForPositionBreakFrequency = imuAgainstKinematicsForPositionBreakFrequency; + this.imuAgainstKinematicsForVelocityBreakFrequency = imuAgainstKinematicsForVelocityBreakFrequency; + this.footAlphaLeakIMUOnly = footAlphaLeakIMUOnly; + this.estimatorDT = estimatorDT; foot = contactableFoot.getRigidBody(); soleFrame = contactableFoot.getContactFrame(); String namePrefix = foot.getName(); - DoubleProvider alphaFoot = () -> AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(footToRootJointPositionBreakFrequency.getValue(), - estimatorDT); + DoubleProvider alphaFoot = () -> AlphaFilterTools.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); + previousCoPInFootFrame = new YoFramePoint2D(namePrefix + "PreviousCoPInFootFrame", soleFrame, registry); - DoubleProvider alphaCop = () -> AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(copFilterBreakFrequency.getValue(), estimatorDT); + 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); - copPositionInWorld = new YoFramePoint3D(namePrefix + "CoPPositionsInWorld", worldFrame, registry); - footVelocityInWorld = new YoFrameVector3D(namePrefix + "VelocityInWorld", worldFrame, registry); + copPositionInWorld = new YoFramePoint3D(namePrefix + "CoPPositionInWorld", worldFrame, registry); + copVelocityInWorld = new YoFrameVector3D(namePrefix + "CoPVelocityInWorld", worldFrame, registry); } public void createVisualization(YoGraphicsListRegistry yoGraphicsListRegistry) @@ -110,12 +153,10 @@ public void createVisualization(YoGraphicsListRegistry yoGraphicsListRegistry) @Override public YoGraphicDefinition getSCS2YoGraphics() { - YoGraphicPoint2DDefinition copVisual = YoGraphicDefinitionFactory.newYoGraphicPoint2D(foot.getName() + "StateEstimatorCoP", - copPositionInWorld, - 0.01, - ColorDefinitions.DeepPink(), - DefaultPoint2DGraphic.CIRCLE); - return copVisual; + return YoGraphicDefinitionFactory.newYoGraphicPoint2D(foot.getName() + "StateEstimatorCoP", copPositionInWorld, + 0.01, + ColorDefinitions.DeepPink(), + DefaultPoint2DGraphic.CIRCLE); } public void initialize() @@ -123,18 +164,67 @@ public void initialize() footToRootJointPosition.reset(); copFilteredInFootFrame.reset(); copFilteredInFootFrame.update(0.0, 0.0); - footVelocityInWorld.setToZero(); + 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 updateKinematics(TwistReadOnly rootBodyTwist) + { + computeFootTwistInWorld(rootBodyTwist); + computeCoPLinearVelocityInWorld(copVelocityInWorld); + + tempFramePoint.setToZero(rootJointFrame); + tempFramePoint.changeFrame(soleFrame); + + tempFrameVector.setIncludingFrame(tempFramePoint); + tempFrameVector.changeFrame(worldFrame); + + footToRootJointPosition.update(tempFrameVector); + } + + /** + * One of the options for computing the + * @param useControllerDesiredCoP + * @param zPosition + */ + public void updateCoPAndFootSettingZ(boolean trustCoPAsNonSlippingContactPoint, boolean useControllerDesiredCoP, double zPosition) + { + updateCoPPosition(trustCoPAsNonSlippingContactPoint, useControllerDesiredCoP); + copPositionInWorld.setZ(zPosition); + correctFootPositionsUsingCoP(trustCoPAsNonSlippingContactPoint); + } + + public void updateCoPAndFootPosition(boolean trustCoPAsNonSlippingContactPoint, boolean useControllerDesiredCoP) + { + updateCoPPosition(trustCoPAsNonSlippingContactPoint, useControllerDesiredCoP); + correctFootPositionsUsingCoP(trustCoPAsNonSlippingContactPoint); + } + + public void setFootZPositionInWorld(double zPosition) + { + footPositionInWorld.setZ(zPosition); + } + + /** * 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 */ - public void updatePelvisWithKinematics(int numberOfTrustedFeet, + public void correctPelvisFromKinematics(int numberOfTrustedFeet, double alphaVelocityUpdate, FixedFramePoint3DBasics rootJointPosition, FixedFrameVector3DBasics rootJointLinearVelocity) @@ -144,63 +234,64 @@ public void updatePelvisWithKinematics(int numberOfTrustedFeet, rootJointPositionPerFoot.add(footPositionInWorld, footToRootJointPosition); rootJointPosition.scaleAdd(scaleFactor, rootJointPositionPerFoot, rootJointPosition); - rootJointLinearVelocity.scaleAdd(-scaleFactor * alphaVelocityUpdate, footVelocityInWorld, rootJointLinearVelocity); + rootJointLinearVelocity.scaleAdd(-scaleFactor * alphaVelocityUpdate, copVelocityInWorld, rootJointLinearVelocity); } /** - * updates the position of a swinging foot + * 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 swingingFoot a foot in swing - * @param pelvisPosition the current pelvis position + * @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 updateUntrustedFootPosition(FramePoint3DReadOnly pelvisPosition) + public void correctFootPositionFromRootJoint(boolean isTrusted, boolean trustCoPAsNonSlippingContactPoint, FramePoint3DReadOnly rootJointPosition) { - footPositionInWorld.sub(pelvisPosition, footToRootJointPosition); + tempPoint.set(footPositionInWorld); + footPositionInWorld.sub(rootJointPosition, footToRootJointPosition); - copPositionInWorld.set(footPositionInWorld); - - copRawInFootFrame.setToZero(); - copFilteredInFootFrame.setToZero(); - } - - private final FrameVector3D tempFrameVector = new FrameVector3D(); - - public void updateTrustedFootPosition(boolean trustCoPAsNonSlippingContactPoint, FramePoint3DReadOnly rootJointPosition) - { - if (trustCoPAsNonSlippingContactPoint) + if (!isTrusted) { - tempFrameVector.setIncludingFrame(rootJointPosition); - tempFrameVector.sub(footToRootJointPosition); // New foot position - tempFrameVector.sub(footPositionInWorld); // Delta from previous to new foot position + copPositionInWorld.set(footPositionInWorld); + copRawInFootFrame.setToZero(); + copFilteredInFootFrame.setToZero(); + } + else if (trustCoPAsNonSlippingContactPoint) + { + tempFrameVector.sub(footPositionInWorld, tempPoint); // Delta from previous to new foot position copPositionInWorld.add(tempFrameVector); // New CoP position } + } + + public FrameVector3DReadOnly getFootToRootJointPosition() + { + return footToRootJointPosition; + } - footPositionInWorld.set(rootJointPosition); - footPositionInWorld.sub(footToRootJointPosition); + public FrameVector3DReadOnly getCopVelocityInWorld() + { + return copVelocityInWorld; } - public void setCoPZPositionInWorld(boolean useControllerDesiredCoP, double zPosition) + public FrameVector3DReadOnly getFootLinearVelocityInWorld() { - updateCoPPosition(true, useControllerDesiredCoP); - copPositionInWorld.setZ(zPosition); - correctFootPositionsUsingCoP(true); + return footFusedLinearVelocityInWorld; } - public void setFootZPositionInWorld(double zPosition) + public FrameVector3DReadOnly getFootAngularVelocityInWorld() { - footPositionInWorld.setZ(zPosition); + return footFusedAngularVelocityInWorld; } private final FramePoint2D tempCoP2d = new FramePoint2D(); private final FrameVector3D tempCoPOffset = new FrameVector3D(); - /** - * Compute the foot CoP. The CoP is the point on the support foot trusted to be not slipping. - * - * @param trustedSide - * @param footSwitch - */ - public void updateCoPPosition(boolean trustCoPAsNonSlippingContactPoint, boolean useControllerDesiredCoP) + private void updateCoPPosition(boolean trustCoPAsNonSlippingContactPoint, boolean useControllerDesiredCoP) { if (trustCoPAsNonSlippingContactPoint) { @@ -243,63 +334,47 @@ public void updateCoPPosition(boolean trustCoPAsNonSlippingContactPoint, boolean } } - copRawInFootFrame.set(tempCoP2d); - tempCoPOffset.setIncludingFrame(soleFrame, copFilteredInFootFrame.getX(), copFilteredInFootFrame.getY(), 0.0); + // Update the CoP values. + previousCoPInFootFrame.set(copFilteredInFootFrame); + copRawInFootFrame.set(tempCoP2d); copFilteredInFootFrame.update(); + + // Update the change in the CoP from the previous tick. tempCoPOffset.setIncludingFrame(soleFrame, - copFilteredInFootFrame.getX() - tempCoPOffset.getX(), - copFilteredInFootFrame.getY() - tempCoPOffset.getY(), + copFilteredInFootFrame.getX() - previousCoPInFootFrame.getX(), + copFilteredInFootFrame.getY() - previousCoPInFootFrame.getY(), 0.0); - tempCoPOffset.changeFrame(worldFrame); + // Apply this same change to the CoP in world. copPositionInWorld.add(tempCoPOffset); } else { tempCoP2d.setToZero(soleFrame); + copRawInFootFrame.setToZero(); + copFilteredInFootFrame.setToZero(); + copPositionInWorld.setFromReferenceFrame(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 - */ - public void correctFootPositionsUsingCoP(boolean trustCoPAsNonSlippingContactPoint) + private void correctFootPositionsUsingCoP(boolean trustCoPAsNonSlippingContactPoint) { if (!trustCoPAsNonSlippingContactPoint) return; + // 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(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); + private final FrameVector3D linearAcceleration = new FrameVector3D(); + private final FrameVector3D angularVelocity = new FrameVector3D(); - footToRootJointPosition.update(tempFrameVector); - } - - private final Twist tempRootBodyTwist = new Twist(); - private final Twist footTwistInWorld = new Twist(); - - public void updateFootLinearVelocityInWorld(TwistReadOnly rootBodyTwist) - { - computeFootLinearVelocityInWorld(rootBodyTwist, footVelocityInWorld); - } - - private void computeFootLinearVelocityInWorld(TwistReadOnly rootBodyTwist, FixedFrameVector3DBasics footLinearVelocityToPack) + private void computeFootTwistInWorld(TwistReadOnly rootBodyTwist) { tempRootBodyTwist.setIncludingFrame(rootBodyTwist); tempRootBodyTwist.setBaseFrame(worldFrame); @@ -310,16 +385,70 @@ private void computeFootLinearVelocityInWorld(TwistReadOnly rootBodyTwist, Fixed footTwistInWorld.setBodyFrame(soleFrame); footTwistInWorld.changeFrame(worldFrame); - footTwistInWorld.getLinearVelocityAt(copPositionInWorld, footLinearVelocityToPack); - } + if (USE_IMU_DATA) + { + // 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()); + } - public FrameVector3DReadOnly getFootToRootJointPosition() - { - return footToRootJointPosition; + // 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()); + } } - public FrameVector3DReadOnly getFootVelocityInWorld() + private void computeCoPLinearVelocityInWorld(FixedFrameVector3DBasics footLinearVelocityToPack) { - return footVelocityInWorld; + footTwistInWorld.getLinearPart().set(footFusedLinearVelocityInWorld); + footTwistInWorld.getAngularPart().set(footFusedAngularVelocityInWorld); + + footTwistInWorld.getLinearVelocityAt(copPositionInWorld, footLinearVelocityToPack); } } From feaa06ddae3ff0e89521d208edade662c3d3bbff Mon Sep 17 00:00:00 2001 From: Robert Date: Thu, 4 Sep 2025 01:36:26 -0500 Subject: [PATCH 14/18] got the IMU based locomotion working better --- ...sKinematicsBasedLinearStateCalculator.java | 60 ++++++++++++++--- .../PelvisLinearStateUpdater.java | 6 +- .../SingleFootEstimator.java | 64 +++++++++++++++---- 3 files changed, 106 insertions(+), 24 deletions(-) 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 845db3663e16..fedc28bc87d2 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 @@ -1,5 +1,6 @@ package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation; +import java.util.ArrayList; import java.util.HashMap; import java.util.List; import java.util.Map; @@ -109,7 +110,7 @@ public PelvisKinematicsBasedLinearStateCalculator(FullInverseDynamicsStructure i YoDouble footAlphaLeakIMUOnly = new YoDouble("footIMUOnlyAlphaLeak", registry); YoDouble imuAgainstKinematicsForVelocityBreakFrequency = new YoDouble("footIMUAgainstKinematicsForVelocityBreakFrequency", registry); YoDouble imuAgainstKinematicsForPositionBreakFrequency = new YoDouble("footIMUAgainstKinematicsForPositionBreakFrequency", registry); - imuAgainstKinematicsForPositionBreakFrequency.set(200.0); + imuAgainstKinematicsForPositionBreakFrequency.set(0.0); imuAgainstKinematicsForVelocityBreakFrequency.set(2.0); footAlphaLeakIMUOnly.set(0.999); @@ -234,14 +235,28 @@ public void updateNoTrustedFeet(FramePoint3DReadOnly pelvisPosition, FrameVector rootJointLinearVelocity.setToZero(); } - public void estimatePelvisLinearState(List trustedFeet, List unTrustedFeet, FramePoint3DReadOnly pelvisPosition) + private final List trustedFeetLocal = new ArrayList<>(); + private final List unTrustedFeetLocal = new ArrayList<>(); + + public void estimatePelvisLinearState(List trustedFeet, + List unTrustedFeet, + FramePoint3DReadOnly pelvisPosition, + FrameVector3DReadOnly pelvisLinearVelocity) { if (!kinematicsIsUpToDate.getBooleanValue()) throw new RuntimeException("Leg kinematics needs to be updated before trying to estimate the pelvis position/linear velocity."); + trustedFeetLocal.clear(); + unTrustedFeetLocal.clear(); + for (int i = 0; i < trustedFeet.size(); i++) + trustedFeetLocal.add(trustedFeet.get(i)); + for (int i = 0; i < unTrustedFeet.size(); i++) + unTrustedFeetLocal.add(unTrustedFeet.get(i)); + + for (int i = 0; i < trustedFeetLocal.size(); i++) { - SingleFootEstimator footEstimator = footEstimatorMap.get(trustedFeet.get(i)); + SingleFootEstimator footEstimator = footEstimatorMap.get(trustedFeetLocal.get(i)); if (assumeTrustedFootAtZeroHeight.getValue()) { if (trustCoPAsNonSlippingContactPoint.getValue()) @@ -257,23 +272,52 @@ public void estimatePelvisLinearState(List trustedFeet, List 0) { updateTrustedFeetLists(); - kinematicsBasedLinearStateCalculator.estimatePelvisLinearState(listOfTrustedFeet, listOfUnTrustedFeet, rootJointPosition); + kinematicsBasedLinearStateCalculator.estimatePelvisLinearState(listOfTrustedFeet, listOfUnTrustedFeet, rootJointPosition, rootJointVelocity); if (imuBasedLinearStateCalculator.isEstimationEnabled()) { 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 index 8b6c0b449991..be9aa14b638e 100644 --- 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 @@ -9,6 +9,7 @@ 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.FramePoint2DReadOnly; import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly; import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; import us.ihmc.euclid.referenceFrame.interfaces.FrameVertex2DSupplier; @@ -39,6 +40,7 @@ import us.ihmc.yoVariables.providers.BooleanProvider; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; class SingleFootEstimator implements SCS2YoGraphicHolder { @@ -57,13 +59,17 @@ class SingleFootEstimator implements SCS2YoGraphicHolder private final YoFrameVector3D footFusedLinearVelocityInWorld; private final YoFrameVector3D footFusedAngularVelocityInWorld; + private final YoBoolean footIsMoving; + private final YoFrameVector3D copVelocityInWorld; private final AlphaFilteredYoFrameVector3D footToRootJointPosition; private final YoFramePoint3D footPositionInWorld; /** Debug variable */ private final YoFramePoint3D rootJointPositionPerFoot; - private final YoFramePoint3D copPositionInWorld; + private final YoFramePoint3D copIKPositionInWorld; + private final YoFramePoint3D copIMUPositionInWorld; + private final YoFramePoint3D copFusedPositionInWorld; private final YoFramePoint2D previousCoPInFootFrame; private final AlphaFilteredYoFramePoint2D copFilteredInFootFrame; @@ -124,6 +130,8 @@ public SingleFootEstimator(FloatingJointBasics rootJoint, copRawInFootFrame = new YoFramePoint2D(namePrefix + "CoPRawInFootFrame", soleFrame, registry); previousCoPInFootFrame = new YoFramePoint2D(namePrefix + "PreviousCoPInFootFrame", soleFrame, 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); @@ -134,7 +142,9 @@ public SingleFootEstimator(FloatingJointBasics rootJoint, DoubleProvider alphaCop = () -> AlphaFilterTools.computeAlphaGivenBreakFrequencyProperly(copFilterBreakFrequency.getValue(), estimatorDT); copFilteredInFootFrame = new AlphaFilteredYoFramePoint2D(namePrefix + "CoPFilteredInFootFrame", "", registry, alphaCop, copRawInFootFrame); copFilteredInFootFrame.update(0.0, 0.0); - copPositionInWorld = new YoFramePoint3D(namePrefix + "CoPPositionInWorld", worldFrame, registry); + 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); } @@ -144,7 +154,7 @@ public void createVisualization(YoGraphicsListRegistry yoGraphicsListRegistry) return; String sidePrefix = foot.getName(); - YoGraphicPosition copInWorld = new YoGraphicPosition(sidePrefix + "StateEstimatorCoP", copPositionInWorld, 0.005, YoAppearance.DeepPink()); + YoGraphicPosition copInWorld = new YoGraphicPosition(sidePrefix + "StateEstimatorCoP", copFusedPositionInWorld, 0.005, YoAppearance.DeepPink()); YoArtifactPosition artifact = copInWorld.createArtifact(); artifact.setVisible(false); yoGraphicsListRegistry.registerArtifact("StateEstimator", artifact); @@ -153,7 +163,7 @@ public void createVisualization(YoGraphicsListRegistry yoGraphicsListRegistry) @Override public YoGraphicDefinition getSCS2YoGraphics() { - return YoGraphicDefinitionFactory.newYoGraphicPoint2D(foot.getName() + "StateEstimatorCoP", copPositionInWorld, + return YoGraphicDefinitionFactory.newYoGraphicPoint2D(foot.getName() + "StateEstimatorCoP", copFusedPositionInWorld, 0.01, ColorDefinitions.DeepPink(), DefaultPoint2DGraphic.CIRCLE); @@ -192,6 +202,12 @@ public void updateKinematics(TwistReadOnly rootBodyTwist) tempFrameVector.changeFrame(worldFrame); footToRootJointPosition.update(tempFrameVector); + + // FIXME remove magic parameters + if (USE_IMU_DATA) + footIsMoving.set(getFootLinearVelocityInWorld().norm() > 0.5 || getFootAngularVelocityInWorld().norm() > 1.0); + else + footIsMoving.set(false); } /** @@ -202,7 +218,7 @@ public void updateKinematics(TwistReadOnly rootBodyTwist) public void updateCoPAndFootSettingZ(boolean trustCoPAsNonSlippingContactPoint, boolean useControllerDesiredCoP, double zPosition) { updateCoPPosition(trustCoPAsNonSlippingContactPoint, useControllerDesiredCoP); - copPositionInWorld.setZ(zPosition); + copFusedPositionInWorld.setZ(zPosition); correctFootPositionsUsingCoP(trustCoPAsNonSlippingContactPoint); } @@ -257,14 +273,14 @@ public void correctFootPositionFromRootJoint(boolean isTrusted, boolean trustCoP if (!isTrusted) { - copPositionInWorld.set(footPositionInWorld); + copFusedPositionInWorld.set(footPositionInWorld); copRawInFootFrame.setToZero(); copFilteredInFootFrame.setToZero(); } else if (trustCoPAsNonSlippingContactPoint) { tempFrameVector.sub(footPositionInWorld, tempPoint); // Delta from previous to new foot position - copPositionInWorld.add(tempFrameVector); // New CoP position + copFusedPositionInWorld.add(tempFrameVector); // New CoP position } } @@ -288,6 +304,11 @@ public FrameVector3DReadOnly getFootAngularVelocityInWorld() return footFusedAngularVelocityInWorld; } + public boolean isFootMoving() + { + return footIsMoving.getBooleanValue(); + } + private final FramePoint2D tempCoP2d = new FramePoint2D(); private final FrameVector3D tempCoPOffset = new FrameVector3D(); @@ -340,6 +361,9 @@ private void updateCoPPosition(boolean trustCoPAsNonSlippingContactPoint, boolea 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(), @@ -347,16 +371,25 @@ private void updateCoPPosition(boolean trustCoPAsNonSlippingContactPoint, boolea 0.0); tempCoPOffset.changeFrame(worldFrame); // Apply this same change to the CoP in world. - copPositionInWorld.add(tempCoPOffset); + copFusedPositionInWorld.add(tempCoPOffset); } else { tempCoP2d.setToZero(soleFrame); - copRawInFootFrame.setToZero(); - copFilteredInFootFrame.setToZero(); - copPositionInWorld.setFromReferenceFrame(soleFrame); + // Update the CoP value to match the center of the foot. + copIKPositionInWorld.setFromReferenceFrame(soleFrame); } + if (USE_IMU_DATA) + { + // 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 + double alpha = AlphaFilterTools.computeAlphaGivenBreakFrequencyProperly(imuAgainstKinematicsForPositionBreakFrequency.getValue(), estimatorDT); + copFusedPositionInWorld.interpolate(copIKPositionInWorld, copIMUPositionInWorld, alpha); + } } private void correctFootPositionsUsingCoP(boolean trustCoPAsNonSlippingContactPoint) @@ -368,7 +401,7 @@ private void correctFootPositionsUsingCoP(boolean trustCoPAsNonSlippingContactPo tempCoPOffset.setIncludingFrame(copFilteredInFootFrame, 0.0); tempCoPOffset.changeFrame(worldFrame); // Update where the foot must be, according to this CoP location - footPositionInWorld.sub(copPositionInWorld, tempCoPOffset); + footPositionInWorld.sub(copFusedPositionInWorld, tempCoPOffset); } private final FrameVector3D linearAcceleration = new FrameVector3D(); @@ -445,10 +478,15 @@ private void computeFootTwistInWorld(TwistReadOnly rootBodyTwist) } private void computeCoPLinearVelocityInWorld(FixedFrameVector3DBasics footLinearVelocityToPack) + { + computeLinearVelocityAtPointInWorld(copFusedPositionInWorld, footLinearVelocityToPack); + } + + private void computeLinearVelocityAtPointInWorld(FramePoint3DReadOnly point, FixedFrameVector3DBasics footLinearVelocityToPack) { footTwistInWorld.getLinearPart().set(footFusedLinearVelocityInWorld); footTwistInWorld.getAngularPart().set(footFusedAngularVelocityInWorld); - footTwistInWorld.getLinearVelocityAt(copPositionInWorld, footLinearVelocityToPack); + footTwistInWorld.getLinearVelocityAt(point, footLinearVelocityToPack); } } From aa2d75f50a13653513a0ab49a5c41a6b8f6d4019 Mon Sep 17 00:00:00 2001 From: Robert Date: Thu, 4 Sep 2025 01:54:46 -0500 Subject: [PATCH 15/18] started passing in a use imu flag --- .../PelvisKinematicsBasedLinearStateCalculator.java | 2 ++ .../SingleFootEstimator.java | 11 ++++++----- 2 files changed, 8 insertions(+), 5 deletions(-) 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 fedc28bc87d2..c93f58ab61a0 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 @@ -107,6 +107,7 @@ public PelvisKinematicsBasedLinearStateCalculator(FullInverseDynamicsStructure i copFilterBreakFrequency = new DoubleParameter("CopFilterBreakFrequency", registry, stateEstimatorParameters.getCoPFilterFreqInHertz()); correctTrustedFeetPositions = new BooleanParameter("correctTrustedFeetPositions", registry, stateEstimatorParameters.correctTrustedFeetPositions()); + YoBoolean useFootIMUData = new YoBoolean("useFootIMUData", registry); YoDouble footAlphaLeakIMUOnly = new YoDouble("footIMUOnlyAlphaLeak", registry); YoDouble imuAgainstKinematicsForVelocityBreakFrequency = new YoDouble("footIMUAgainstKinematicsForVelocityBreakFrequency", registry); YoDouble imuAgainstKinematicsForPositionBreakFrequency = new YoDouble("footIMUAgainstKinematicsForPositionBreakFrequency", registry); @@ -133,6 +134,7 @@ public PelvisKinematicsBasedLinearStateCalculator(FullInverseDynamicsStructure i centerOfPressureDataHolderFromController, cancelGravityFromAccelerationMeasurement, gravityVector, + useFootIMUData, imuAgainstKinematicsForPositionBreakFrequency, imuAgainstKinematicsForVelocityBreakFrequency, footAlphaLeakIMUOnly, 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 index be9aa14b638e..8dd07ba743ea 100644 --- 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 @@ -44,14 +44,13 @@ class SingleFootEstimator implements SCS2YoGraphicHolder { - private static boolean USE_IMU_DATA = true; - 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; @@ -99,6 +98,7 @@ public SingleFootEstimator(FloatingJointBasics rootJoint, CenterOfPressureDataHolder centerOfPressureDataHolderFromController, BooleanProvider cancelGravityFromAccelerationMeasurement, FrameVector3DReadOnly gravityVector, + BooleanProvider useIMUData, DoubleProvider imuAgainstKinematicsForPositionBreakFrequency, DoubleProvider imuAgainstKinematicsForVelocityBreakFrequency, DoubleProvider footAlphaLeakIMUOnly, @@ -112,6 +112,7 @@ public SingleFootEstimator(FloatingJointBasics rootJoint, this.centerOfPressureDataHolderFromController = centerOfPressureDataHolderFromController; this.cancelGravityFromAccelerationMeasurement = cancelGravityFromAccelerationMeasurement; this.gravityVector = gravityVector; + this.useIMUData = useIMUData; this.imuAgainstKinematicsForPositionBreakFrequency = imuAgainstKinematicsForPositionBreakFrequency; this.imuAgainstKinematicsForVelocityBreakFrequency = imuAgainstKinematicsForVelocityBreakFrequency; this.footAlphaLeakIMUOnly = footAlphaLeakIMUOnly; @@ -204,7 +205,7 @@ public void updateKinematics(TwistReadOnly rootBodyTwist) footToRootJointPosition.update(tempFrameVector); // FIXME remove magic parameters - if (USE_IMU_DATA) + if (useIMUData.getValue() && footIMU != null) footIsMoving.set(getFootLinearVelocityInWorld().norm() > 0.5 || getFootAngularVelocityInWorld().norm() > 1.0); else footIsMoving.set(false); @@ -380,7 +381,7 @@ private void updateCoPPosition(boolean trustCoPAsNonSlippingContactPoint, boolea copIKPositionInWorld.setFromReferenceFrame(soleFrame); } - if (USE_IMU_DATA) + if (useIMUData.getValue() && footIMU != null) { // Integrate the fused position using the velocity, which is from the kinematics and the IMU. computeLinearVelocityAtPointInWorld(copFusedPositionInWorld, tempFrameVector); @@ -418,7 +419,7 @@ private void computeFootTwistInWorld(TwistReadOnly rootBodyTwist) footTwistInWorld.setBodyFrame(soleFrame); footTwistInWorld.changeFrame(worldFrame); - if (USE_IMU_DATA) + if (useIMUData.getValue() && footIMU != null) { // Record the kinematic data footIKLinearVelocityInWorld.set(footTwistInWorld.getLinearPart()); From cb153e89673d642b718250a079e724cd1ce33f08 Mon Sep 17 00:00:00 2001 From: Robert Date: Tue, 16 Sep 2025 16:40:00 -0500 Subject: [PATCH 16/18] refactor where the foot estimator states exist --- ...sKinematicsBasedLinearStateCalculator.java | 285 ++++-------------- .../PelvisLinearStateUpdater.java | 228 ++++++++++++-- .../SingleFootEstimator.java | 83 +++-- 3 files changed, 301 insertions(+), 295 deletions(-) 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 c93f58ab61a0..1c74f54ef57f 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 @@ -1,33 +1,25 @@ package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation; -import java.util.ArrayList; import java.util.HashMap; import java.util.List; import java.util.Map; import us.ihmc.euclid.referenceFrame.FramePoint3D; -import us.ihmc.euclid.referenceFrame.FrameVector3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly; import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; -import us.ihmc.euclid.referenceFrame.tools.EuclidFrameFactories; -import us.ihmc.euclid.tuple3D.Vector3D; import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry; import us.ihmc.humanoidRobotics.model.CenterOfPressureDataHolder; -import us.ihmc.mecano.multiBodySystem.interfaces.FloatingJointBasics; import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; -import us.ihmc.mecano.spatial.Twist; -import us.ihmc.robotics.SCS2YoGraphicHolder; import us.ihmc.robotics.contactable.ContactablePlaneBody; import us.ihmc.sensorProcessing.stateEstimation.IMUSensorReadOnly; import us.ihmc.yoVariables.euclid.filters.BacklashCompensatingVelocityYoFrameVector3D; import us.ihmc.robotics.sensors.FootSwitchInterface; -import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinition; -import us.ihmc.scs2.definition.yoGraphic.YoGraphicGroupDefinition; import us.ihmc.sensorProcessing.stateEstimation.StateEstimatorParameters; import us.ihmc.sensorProcessing.stateEstimation.evaluation.FullInverseDynamicsStructure; 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; @@ -43,16 +35,11 @@ * * @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(); @@ -60,93 +47,39 @@ public class PelvisKinematicsBasedLinearStateCalculator implements SCS2YoGraphic private final YoFrameVector3D rootJointLinearVelocity = new YoFrameVector3D("estimatedRootJointLinearVelocityKinematics", 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, - Map footIMUs, - IMUBiasProvider imuBiasProvider, - CenterOfPressureDataHolder centerOfPressureDataHolderFromController, - BooleanProvider cancelGravityFromAccelerationMeasurement, + public PelvisKinematicsBasedLinearStateCalculator(Map footEstimatorMap, double estimatorDT, - double gravitationalAcceleration, 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()); - - YoBoolean useFootIMUData = new YoBoolean("useFootIMUData", registry); - YoDouble footAlphaLeakIMUOnly = new YoDouble("footIMUOnlyAlphaLeak", registry); - YoDouble imuAgainstKinematicsForVelocityBreakFrequency = new YoDouble("footIMUAgainstKinematicsForVelocityBreakFrequency", registry); - YoDouble imuAgainstKinematicsForPositionBreakFrequency = new YoDouble("footIMUAgainstKinematicsForPositionBreakFrequency", registry); - imuAgainstKinematicsForPositionBreakFrequency.set(0.0); - imuAgainstKinematicsForVelocityBreakFrequency.set(2.0); - footAlphaLeakIMUOnly.set(0.999); - - FrameVector3DReadOnly gravityVector = EuclidFrameFactories.newLinkedFrameVector3DReadOnly(() -> worldFrame, new Vector3D(0, 0, -Math.abs(gravitationalAcceleration))); - - 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, - gravityVector, - useFootIMUData, - imuAgainstKinematicsForPositionBreakFrequency, - imuAgainstKinematicsForVelocityBreakFrequency, - footAlphaLeakIMUOnly, - 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", "", @@ -159,41 +92,26 @@ 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.correctFootPositionFromRootJoint(false, false, pelvisPosition); - } kinematicsIsUpToDate.set(false); } - private final Twist tempRootBodyTwist = new Twist(); /** * Updates the different kinematics related stuff that is used to estimate the pelvis state @@ -201,91 +119,58 @@ public void initialize(FramePoint3DReadOnly pelvisPosition) public void updateKinematics() { rootJointPosition.setToZero(); - - tempRootBodyTwist.setIncludingFrame(rootJoint.getJointTwist()); - tempRootBodyTwist.getLinearPart().setMatchingFrame(rootJointLinearVelocity); - - for (SingleFootEstimator footEstimator : footEstimators) - { - footEstimator.updateKinematics(tempRootBodyTwist); - } - kinematicsIsUpToDate.set(true); } - /** * 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.correctFootPositionFromRootJoint(false, false, pelvisPosition); - } - - rootJointPosition.set(pelvisPosition); + this.rootJointPosition.set(rootJointPosition); - if (pelvisLinearVelocity != null) - rootJointLinearVelocity.set(pelvisLinearVelocity); + if (rootJointLinearVelocity != null) + this.rootJointLinearVelocity.set(rootJointLinearVelocity); else - rootJointLinearVelocity.setToZero(); + this.rootJointLinearVelocity.setToZero(); } - private final List trustedFeetLocal = new ArrayList<>(); - private final List unTrustedFeetLocal = new ArrayList<>(); + public boolean getKinematicsIsUpToDate() + { + return kinematicsIsUpToDate.getBooleanValue(); + } - public void estimatePelvisLinearState(List trustedFeet, - List unTrustedFeet, - FramePoint3DReadOnly pelvisPosition, - FrameVector3DReadOnly pelvisLinearVelocity) + public boolean getAssumeTrustedFootAtZeroHeight() { - if (!kinematicsIsUpToDate.getBooleanValue()) - throw new RuntimeException("Leg kinematics needs to be updated before trying to estimate the pelvis position/linear velocity."); + return assumeTrustedFootAtZeroHeight.getValue(); + } - trustedFeetLocal.clear(); - unTrustedFeetLocal.clear(); + public boolean getTrustCoPAsNonSlippingContactPoint() + { + return trustCoPAsNonSlippingContactPoint.getValue(); + } - for (int i = 0; i < trustedFeet.size(); i++) - trustedFeetLocal.add(trustedFeet.get(i)); - for (int i = 0; i < unTrustedFeet.size(); i++) - unTrustedFeetLocal.add(unTrustedFeet.get(i)); + public boolean getUseControllerDesiredCoP() + { + return useControllerDesiredCoP.getValue(); + } - for (int i = 0; i < trustedFeetLocal.size(); i++) - { - SingleFootEstimator footEstimator = footEstimatorMap.get(trustedFeetLocal.get(i)); - if (assumeTrustedFootAtZeroHeight.getValue()) - { - if (trustCoPAsNonSlippingContactPoint.getValue()) - { - footEstimator.updateCoPAndFootSettingZ(trustCoPAsNonSlippingContactPoint.getValue(), useControllerDesiredCoP.getValue(), 0.0); - } - else - { - footEstimator.setFootZPositionInWorld(0.0); - } - } - else - { - footEstimator.updateCoPAndFootPosition(trustCoPAsNonSlippingContactPoint.getValue(), useControllerDesiredCoP.getValue()); - } - } + public boolean getCorrectTrustedFeetPositions() + { + return correctTrustedFeetPositions.getValue(); + } - int trustIdx = 0; - while (trustIdx < trustedFeetLocal.size()) - { - if (footEstimatorMap.get(trustedFeetLocal.get(trustIdx)).isFootMoving()) - unTrustedFeetLocal.add(trustedFeetLocal.remove(trustIdx)); - else - trustIdx++; - } + public void estimatePelvisLinearState(List trustedFeet, FramePoint3DReadOnly pelvisPosition, FrameVector3DReadOnly pelvisLinearVelocity) + { + if (!kinematicsIsUpToDate.getBooleanValue()) + throw new RuntimeException("Leg kinematics needs to be updated before trying to estimate the pelvis position/linear velocity."); - if (trustedFeetLocal.isEmpty()) + if (trustedFeet.isEmpty()) { rootJointPosition.set(pelvisPosition); if (pelvisLinearVelocity != null) @@ -295,56 +180,24 @@ public void estimatePelvisLinearState(List trustedFeet, } else { - for (int i = 0; i < trustedFeetLocal.size(); i++) + for (int i = 0; i < trustedFeet.size(); i++) { - SingleFootEstimator footEstimator = footEstimatorMap.get(trustedFeetLocal.get(i)); - footEstimator.correctPelvisFromKinematics(trustedFeetLocal.size(), - alphaRootJointLinearVelocity.getValue(), - rootJointPosition, - rootJointLinearVelocity); - } - } + SingleFootEstimator footEstimator = footEstimatorMap.get(trustedFeet.get(i)); - rootJointLinearVelocityFDDebug.update(); + double scaleFactor = 1.0 / trustedFeet.size(); - // TODO should this use the local copies? - if (correctTrustedFeetPositions.getValue()) - { - for (int i = 0; i < trustedFeetLocal.size(); i++) - { - SingleFootEstimator footEstimator = footEstimatorMap.get(trustedFeetLocal.get(i)); - footEstimator.correctFootPositionFromRootJoint(true, trustCoPAsNonSlippingContactPoint.getValue(), rootJointPosition); + rootJointPosition.scaleAdd(scaleFactor, footEstimator.getRootJointPositionFromKinematics(), rootJointPosition); + rootJointLinearVelocity.scaleAdd(-scaleFactor * alphaRootJointLinearVelocity.getValue(), + footEstimator.getCopVelocityInWorld(), + rootJointLinearVelocity); } } - for (int i = 0; i < unTrustedFeetLocal.size(); i++) - { - SingleFootEstimator footEstimator = footEstimatorMap.get(unTrustedFeetLocal.get(i)); - footEstimator.correctFootPositionFromRootJoint(false, false, pelvisPosition); - } + rootJointLinearVelocityFDDebug.update(); kinematicsIsUpToDate.set(false); } - public void setPelvisPosition(FramePoint3DReadOnly pelvisPosition) - { - rootJointPosition.set(pelvisPosition); - } - - public void setPelvisLinearVelocity(FrameVector3DReadOnly pelvisLinearVelocity) - { - rootJointLinearVelocityFDDebug.reset(); - rootJointLinearVelocityFDDebug.set(pelvisLinearVelocity); - rootJointLinearVelocity.set(pelvisLinearVelocity); - } - - public void setPelvisLinearVelocityToZero() - { - rootJointLinearVelocityFDDebug.reset(); - rootJointLinearVelocityFDDebug.setToZero(); - rootJointLinearVelocity.setToZero(); - } - public FramePoint3DReadOnly getPelvisPosition() { return rootJointPosition; @@ -354,28 +207,4 @@ public FrameVector3DReadOnly getPelvisVelocity() { return rootJointLinearVelocity; } - - public void getFootToRootJointPosition(FramePoint3D positionToPack, RigidBodyBasics foot) - { - positionToPack.setIncludingFrame(footEstimatorMap.get(foot).getFootToRootJointPosition()); - } - - public FrameVector3DReadOnly getFootVelocityInWorld(RigidBodyBasics foot) - { - return footEstimatorMap.get(foot).getCopVelocityInWorld(); - } - - @Override - public YoGraphicDefinition getSCS2YoGraphics() - { - if (!VISUALIZE) - return null; - - YoGraphicGroupDefinition group = new YoGraphicGroupDefinition(getClass().getSimpleName()); - for (SingleFootEstimator footEstimator : footEstimators) - { - group.addChild(footEstimator.getSCS2YoGraphics()); - } - return group; - } } 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 dd3237dc2b03..055f1a34a340 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 @@ -12,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; @@ -57,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; @@ -95,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<>(); @@ -174,17 +182,58 @@ public PelvisLinearStateUpdater(FullInverseDynamicsStructure inverseDynamicsStru footIMUs.put(imu.getMeasurementLink(), imu); } - kinematicsBasedLinearStateCalculator = new PelvisKinematicsBasedLinearStateCalculator(inverseDynamicsStructure, - feetContactablePlaneBodies, - footSwitches, - footIMUs, - imuBiasProvider, - centerOfPressureDataHolderFromController, - cancelGravityFromAccelerationMeasurement, + 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 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); + + 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, + 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, - gravitationalAcceleration, stateEstimatorParameters, - yoGraphicsListRegistry, registry); imuBasedLinearStateCalculator = new PelvisIMUBasedLinearStateCalculator(rootJoint, @@ -275,8 +324,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); @@ -334,6 +386,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(); @@ -352,7 +414,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(); @@ -362,14 +430,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; - numberOfEndEffectorsTrusted.set(setTrustedFeetUsingFootSwitches()); + numberOfEndEffectorsTrusted.set(setTrustedFeetUsingFootSwitchesAndKinematics()); numberOfEndEffectorsFilteredByLoad.set(0); if (numberOfEndEffectorsTrusted.getIntegerValue() >= optimalNumberOfTrustedFeet.getValue()) @@ -405,15 +510,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, rootJointVelocity); + updateTrustedFeet(); + kinematicsBasedLinearStateCalculator.estimatePelvisLinearState(listOfTrustedFeet, rootJointPosition, rootJointVelocity); if (imuBasedLinearStateCalculator.isEstimationEnabled()) { @@ -424,6 +535,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 { @@ -442,8 +556,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); @@ -452,19 +567,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 - haveFeetHitGroundFiltered.get(foot).set(false); + hasFootHitGround.set(false); + if (!footEstimatorMap.get(foot).isFootMoving()) + isFootNotMoving.update(true); + else + 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()) { @@ -478,6 +623,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 @@ -506,6 +653,8 @@ private int setTrustedFeetUsingFootSwitches() } } + + if (numberOfEndEffectorsTrusted == 0) { if (trustImuWhenNoFeetAreInContact.getValue()) @@ -690,7 +839,7 @@ private void computePositionFromMergingMeasurements() } } - private void updateTrustedFeetLists() + private void updateTrustedFeet() { listOfTrustedFeet.clear(); listOfUnTrustedFeet.clear(); @@ -703,6 +852,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) @@ -724,8 +899,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 index 8dd07ba743ea..4500d1c461ef 100644 --- 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 @@ -6,10 +6,8 @@ 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.FramePoint2DReadOnly; import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly; import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; import us.ihmc.euclid.referenceFrame.interfaces.FrameVertex2DSupplier; @@ -42,7 +40,7 @@ import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; -class SingleFootEstimator implements SCS2YoGraphicHolder +public class SingleFootEstimator implements SCS2YoGraphicHolder { private final RigidBodyBasics foot; @@ -175,6 +173,7 @@ public void initialize() footToRootJointPosition.reset(); copFilteredInFootFrame.reset(); copFilteredInFootFrame.update(0.0, 0.0); + copFusedPositionInWorld.setMatchingFrame(copFilteredInFootFrame, 0.0); copVelocityInWorld.setToZero(); } @@ -191,11 +190,11 @@ public void initialize() /** * 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 updateKinematics(TwistReadOnly rootBodyTwist) + public void updateStateFromKinematics(TwistReadOnly rootBodyTwist) { - computeFootTwistInWorld(rootBodyTwist); - computeCoPLinearVelocityInWorld(copVelocityInWorld); + computeFootTwistInWorldFusingBaseStateAndFootIMU(rootBodyTwist); + // Update the vector from the root joint to the foot, based on the kinematics tempFramePoint.setToZero(rootJointFrame); tempFramePoint.changeFrame(soleFrame); @@ -216,42 +215,33 @@ public void updateKinematics(TwistReadOnly rootBodyTwist) * @param useControllerDesiredCoP * @param zPosition */ - public void updateCoPAndFootSettingZ(boolean trustCoPAsNonSlippingContactPoint, boolean useControllerDesiredCoP, double zPosition) + public void computeStateSettingCoPZ(boolean trustCoPAsNonSlippingContactPoint, boolean useControllerDesiredCoP, double zPosition) { - updateCoPPosition(trustCoPAsNonSlippingContactPoint, useControllerDesiredCoP); + updateCoPState(trustCoPAsNonSlippingContactPoint, useControllerDesiredCoP); copFusedPositionInWorld.setZ(zPosition); - correctFootPositionsUsingCoP(trustCoPAsNonSlippingContactPoint); - } + if (trustCoPAsNonSlippingContactPoint) + correctFootPositionUsingCoP(); - public void updateCoPAndFootPosition(boolean trustCoPAsNonSlippingContactPoint, boolean useControllerDesiredCoP) - { - updateCoPPosition(trustCoPAsNonSlippingContactPoint, useControllerDesiredCoP); - correctFootPositionsUsingCoP(trustCoPAsNonSlippingContactPoint); + rootJointPositionPerFoot.add(footPositionInWorld, footToRootJointPosition); } - public void setFootZPositionInWorld(double zPosition) + public void computeState(boolean trustCoPAsNonSlippingContactPoint, boolean useControllerDesiredCoP) { - footPositionInWorld.setZ(zPosition); - } + updateCoPState(trustCoPAsNonSlippingContactPoint, useControllerDesiredCoP); + if (trustCoPAsNonSlippingContactPoint) + correctFootPositionUsingCoP(); + rootJointPositionPerFoot.add(footPositionInWorld, footToRootJointPosition); + } - /** - * 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 - */ - public void correctPelvisFromKinematics(int numberOfTrustedFeet, - double alphaVelocityUpdate, - FixedFramePoint3DBasics rootJointPosition, - FixedFrameVector3DBasics rootJointLinearVelocity) + public void computeStateSettingFootZ(double zPosition) { - double scaleFactor = 1.0 / numberOfTrustedFeet; + footPositionInWorld.setZ(zPosition); + // Update the CoP position from this foot position + copIKPositionInWorld.set(footPositionInWorld); + copFusedPositionInWorld.set(footPositionInWorld); rootJointPositionPerFoot.add(footPositionInWorld, footToRootJointPosition); - rootJointPosition.scaleAdd(scaleFactor, rootJointPositionPerFoot, rootJointPosition); - - rootJointLinearVelocity.scaleAdd(-scaleFactor * alphaVelocityUpdate, copVelocityInWorld, rootJointLinearVelocity); } /** @@ -269,25 +259,30 @@ public void correctPelvisFromKinematics(int numberOfTrustedFeet, */ 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 FrameVector3DReadOnly getFootToRootJointPosition() + public FramePoint3DReadOnly getRootJointPositionFromKinematics() { - return footToRootJointPosition; + return rootJointPositionPerFoot; } public FrameVector3DReadOnly getCopVelocityInWorld() @@ -313,7 +308,7 @@ public boolean isFootMoving() private final FramePoint2D tempCoP2d = new FramePoint2D(); private final FrameVector3D tempCoPOffset = new FrameVector3D(); - private void updateCoPPosition(boolean trustCoPAsNonSlippingContactPoint, boolean useControllerDesiredCoP) + private void updateCoPState(boolean trustCoPAsNonSlippingContactPoint, boolean useControllerDesiredCoP) { if (trustCoPAsNonSlippingContactPoint) { @@ -379,25 +374,28 @@ private void updateCoPPosition(boolean trustCoPAsNonSlippingContactPoint, boolea 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 + // 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 correctFootPositionsUsingCoP(boolean trustCoPAsNonSlippingContactPoint) + private void correctFootPositionUsingCoP() { - if (!trustCoPAsNonSlippingContactPoint) - return; - // Compute where the CoP position is in the world, according to the kinematics tempCoPOffset.setIncludingFrame(copFilteredInFootFrame, 0.0); tempCoPOffset.changeFrame(worldFrame); @@ -408,7 +406,7 @@ private void correctFootPositionsUsingCoP(boolean trustCoPAsNonSlippingContactPo private final FrameVector3D linearAcceleration = new FrameVector3D(); private final FrameVector3D angularVelocity = new FrameVector3D(); - private void computeFootTwistInWorld(TwistReadOnly rootBodyTwist) + private void computeFootTwistInWorldFusingBaseStateAndFootIMU(TwistReadOnly rootBodyTwist) { tempRootBodyTwist.setIncludingFrame(rootBodyTwist); tempRootBodyTwist.setBaseFrame(worldFrame); @@ -478,11 +476,6 @@ private void computeFootTwistInWorld(TwistReadOnly rootBodyTwist) } } - private void computeCoPLinearVelocityInWorld(FixedFrameVector3DBasics footLinearVelocityToPack) - { - computeLinearVelocityAtPointInWorld(copFusedPositionInWorld, footLinearVelocityToPack); - } - private void computeLinearVelocityAtPointInWorld(FramePoint3DReadOnly point, FixedFrameVector3DBasics footLinearVelocityToPack) { footTwistInWorld.getLinearPart().set(footFusedLinearVelocityInWorld); From a6d21f259f774e03eefac8bd7758958f61db1b1c Mon Sep 17 00:00:00 2001 From: Robert Date: Tue, 16 Sep 2025 17:26:19 -0500 Subject: [PATCH 17/18] did some cleanup --- .../PelvisLinearStateUpdater.java | 6 ++++++ .../SingleFootEstimator.java | 17 +++++++++++++++-- 2 files changed, 21 insertions(+), 2 deletions(-) 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 055f1a34a340..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 @@ -189,12 +189,16 @@ public PelvisLinearStateUpdater(FullInverseDynamicsStructure inverseDynamicsStru 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]); @@ -213,6 +217,8 @@ public PelvisLinearStateUpdater(FullInverseDynamicsStructure inverseDynamicsStru copFilterBreakFrequency, centerOfPressureDataHolderFromController, cancelGravityFromAccelerationMeasurement, + footLinearVelocityMovingThreshold, + footAngularVelocityMovingThreshold, gravityVector, useFootIMUData, footImuAgainstKinematicsForPositionBreakFrequency, 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 index 4500d1c461ef..4b70937e0978 100644 --- 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 @@ -39,6 +39,7 @@ 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 { @@ -56,6 +57,8 @@ public class SingleFootEstimator implements SCS2YoGraphicHolder private final YoFrameVector3D footFusedLinearVelocityInWorld; private final YoFrameVector3D footFusedAngularVelocityInWorld; + private final YoDouble footLinearVelocity; + private final YoDouble footAngularVelocity; private final YoBoolean footIsMoving; private final YoFrameVector3D copVelocityInWorld; @@ -81,6 +84,8 @@ public class SingleFootEstimator implements SCS2YoGraphicHolder 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; @@ -95,6 +100,8 @@ public SingleFootEstimator(FloatingJointBasics rootJoint, DoubleProvider copFilterBreakFrequency, CenterOfPressureDataHolder centerOfPressureDataHolderFromController, BooleanProvider cancelGravityFromAccelerationMeasurement, + DoubleProvider footLinearVelocityMovingThreshold, + DoubleProvider footAngularVelocityMovingThreshold, FrameVector3DReadOnly gravityVector, BooleanProvider useIMUData, DoubleProvider imuAgainstKinematicsForPositionBreakFrequency, @@ -109,6 +116,8 @@ public SingleFootEstimator(FloatingJointBasics rootJoint, this.imuBiasProvider = imuBiasProvider; this.centerOfPressureDataHolderFromController = centerOfPressureDataHolderFromController; this.cancelGravityFromAccelerationMeasurement = cancelGravityFromAccelerationMeasurement; + this.footLinearVelocityMovingThreshold = footLinearVelocityMovingThreshold; + this.footAngularVelocityMovingThreshold = footAngularVelocityMovingThreshold; this.gravityVector = gravityVector; this.useIMUData = useIMUData; this.imuAgainstKinematicsForPositionBreakFrequency = imuAgainstKinematicsForPositionBreakFrequency; @@ -129,6 +138,8 @@ public SingleFootEstimator(FloatingJointBasics rootJoint, 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); @@ -203,9 +214,11 @@ public void updateStateFromKinematics(TwistReadOnly rootBodyTwist) footToRootJointPosition.update(tempFrameVector); - // FIXME remove magic parameters + footLinearVelocity.set(getFootLinearVelocityInWorld().norm()); + footAngularVelocity.set(getFootAngularVelocityInWorld().norm()); + if (useIMUData.getValue() && footIMU != null) - footIsMoving.set(getFootLinearVelocityInWorld().norm() > 0.5 || getFootAngularVelocityInWorld().norm() > 1.0); + footIsMoving.set(footLinearVelocity.getDoubleValue() > footLinearVelocityMovingThreshold.getValue() || footAngularVelocity.getDoubleValue() > footAngularVelocityMovingThreshold.getValue()); else footIsMoving.set(false); } From bd9395c1ad3aecb0e571e624cc3d177c4f01771c Mon Sep 17 00:00:00 2001 From: Robert Date: Tue, 16 Sep 2025 17:54:27 -0500 Subject: [PATCH 18/18] started improving the update to the foot state for the kinematic pelvis linear velocity --- ...isKinematicsBasedLinearStateCalculator.java | 13 ++++++++++--- .../SingleFootEstimator.java | 18 ++++++++++++++++++ 2 files changed, 28 insertions(+), 3 deletions(-) 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 1c74f54ef57f..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 @@ -45,6 +45,7 @@ public class PelvisKinematicsBasedLinearStateCalculator 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 */ @@ -180,17 +181,23 @@ public void estimatePelvisLinearState(List trustedFeet, FramePo } else { + averagedTrustedFootVelocity.setToZero(); for (int i = 0; i < trustedFeet.size(); i++) { SingleFootEstimator footEstimator = footEstimatorMap.get(trustedFeet.get(i)); double scaleFactor = 1.0 / trustedFeet.size(); + // The root joint position is zeroed when we call #updateKinematics rootJointPosition.scaleAdd(scaleFactor, footEstimator.getRootJointPositionFromKinematics(), rootJointPosition); - rootJointLinearVelocity.scaleAdd(-scaleFactor * alphaRootJointLinearVelocity.getValue(), - footEstimator.getCopVelocityInWorld(), - rootJointLinearVelocity); + averagedTrustedFootVelocity.scaleAdd(scaleFactor, footEstimator.getCopVelocityInWorld(), averagedTrustedFootVelocity); + } + + // 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); } rootJointLinearVelocityFDDebug.update(); 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 index 4b70937e0978..5d149fcd4f6e 100644 --- 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 @@ -63,9 +63,11 @@ public class SingleFootEstimator implements SCS2YoGraphicHolder 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; @@ -131,7 +133,9 @@ public SingleFootEstimator(FloatingJointBasics rootJoint, 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); @@ -182,6 +186,7 @@ public YoGraphicDefinition getSCS2YoGraphics() public void initialize() { footToRootJointPosition.reset(); + footToRootJointVelocity.reset(); copFilteredInFootFrame.reset(); copFilteredInFootFrame.update(0.0, 0.0); copFusedPositionInWorld.setMatchingFrame(copFilteredInFootFrame, 0.0); @@ -236,6 +241,7 @@ public void computeStateSettingCoPZ(boolean trustCoPAsNonSlippingContactPoint, b correctFootPositionUsingCoP(); rootJointPositionPerFoot.add(footPositionInWorld, footToRootJointPosition); + rootJointVelocityPerFoot.sub(footFusedLinearVelocityInWorld, footToRootJointVelocity); } public void computeState(boolean trustCoPAsNonSlippingContactPoint, boolean useControllerDesiredCoP) @@ -245,6 +251,7 @@ public void computeState(boolean trustCoPAsNonSlippingContactPoint, boolean useC correctFootPositionUsingCoP(); rootJointPositionPerFoot.add(footPositionInWorld, footToRootJointPosition); + rootJointVelocityPerFoot.sub(footFusedLinearVelocityInWorld, footToRootJointVelocity); } public void computeStateSettingFootZ(double zPosition) @@ -255,6 +262,7 @@ public void computeStateSettingFootZ(double zPosition) copFusedPositionInWorld.set(footPositionInWorld); rootJointPositionPerFoot.add(footPositionInWorld, footToRootJointPosition); + rootJointVelocityPerFoot.sub(footFusedLinearVelocityInWorld, footToRootJointVelocity); } /** @@ -298,6 +306,11 @@ public FramePoint3DReadOnly getRootJointPositionFromKinematics() return rootJointPositionPerFoot; } + public FrameVector3DReadOnly getRootJointVelocityFromKinematics() + { + return rootJointVelocityPerFoot; + } + public FrameVector3DReadOnly getCopVelocityInWorld() { return copVelocityInWorld; @@ -426,6 +439,11 @@ private void computeFootTwistInWorldFusingBaseStateAndFootIMU(TwistReadOnly root 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);