Localization - the Kalman Filter

and are the Jacobian matrices

Assume is a

from robot coordinate to inertial coordinate

red: obs
blue: model prediction
green: fused info