Questions about the EKF benchmark experiment

[shirleyanan]

Hello, authors:
Thank you for your contributions. Here are some questions that need your help.

1. The initial states vector contains position and Euler angles, is this calculated velocity and acceleration in a way that is suitable for Euler angles for state updates when using the predict_next_state() function to predict? I think this way of calculating speed is linear speed, not angular speed. Can you clarify the principle about the Eulerian angular update in states?

 def predict_next_state(x: np.ndarray, dt: np.float32):
        """Returns a numpy array of the predicted states for a given state vector x and time delta dt.
        """
        acc_prediction = x[acc_idx:]
        vel_prediction = x[vel_idx:acc_idx] + dt*acc_prediction
        pos_prediction = x[:vel_idx] + dt*vel_prediction + (0.5*dt**2)*acc_prediction

        return np.concatenate([pos_prediction, vel_prediction, acc_prediction]).astype(np.float32)

I look forward to your answers, thanks!

[amirpatel]

For the angle parts of the state vector, we have defined it to be the Euler angles as well as their first- and second-time derivatives. So, we can simply integrate them to predict their next values.

If we instead needed an angular velocity vector e.g. If we want to calculate the Kinetic energy for mechanics or wanted to incorporate a gyro measurement, then we would need a Euler rates matrix (a nonlinear mapping) to use the Euler angle time derivatives to get the angular velocity vector.

See: https://web.stanford.edu/group/ctr/Summer/SP12/02.05_saeedi.pdf section 5.2 for more details.