How to compute the measurement Jacobian in EKF fusion.

[xhniu]

Hi, I have some problems with the sensor fusion function;

How is this formula derived? Can anyone provide some references? Thanks.

void SensorFusionEkf::ComputeMeasurementJacobian() { for (int dof = 0; dof < 3; dof++) { Vector3 delta = Vector3::Zero(); delta[dof] = kFiniteDifferencingEpsilon;

const Rotation epsilon_rotation = RotationFromVector(delta);
const Vector3 delta_rotation = ComputeInnovation(
    epsilon_rotation * current_state_.sensor_from_start_rotation);

const Vector3 col =
    (innovation_ - delta_rotation) / kFiniteDifferencingEpsilon;
accelerometer_measurement_jacobian_(0, dof) = col[0];
accelerometer_measurement_jacobian_(1, dof) = col[1];
accelerometer_measurement_jacobian_(2, dof) = col[2];

} }

[arilow]

Thanks @xhniu for reaching out to us! And sorry for the delay.

Cardboard offers an Extended Kalman Filter implementation to estimate the head pose. The model is intrinsically not linear so a first order Taylor approximation is required (that is why it is called Extended Kalman Filter). In the normal filter formulation, the matrix H is the observation model and it is represented by the Jacobian of the acceleration measurement. The acceleration measurement is a 3D vector and its Jacobian is a 3x3 matrix. This function computes the partial derivative of the accelerometer measurement applied to the observed model with respect to each coordinate (each member of the 3D vector). To do so, it uses an epsilon perturbation ( kFiniteDifferencingEpsilon ) on each coordinate to compute the derivative.

To make it simple: f_prime(x) --> [ f(x + h) - f(x)] / h , repeat this for each acceleration coordinate and you will get the Jacobian.