Observation Model Jacobian for Fixed Transforms

Question

Let's say I have a hypothetical sensor that provides, for example, velocity estimates, and I affix that sensor at some non-zero rotational offset from the robot's base. I also have an EKF that is estimating the robot's velocity.

Normally, the innovation calculation for an EKF looks like this:

$$ y_k = z_k - h(x_k) $$

In this case, $h$ would just be the rotation matrix of the rotational offset. What are the ramifications if instead, I pre-process the sensor measurement by rotating $z_k$ by the inverse rotation, which will put its coordinates in the frame of the robot? Can I then safely just make $h$ the identity matrix $I$?

Answer 1

The KF (EKF), even though it is closed form, does not require you to input directly what your sensors sense. Preprocessing is OK if you are doing it perfectly and not losing information.

Consider GPS measurements, which may occur in a coordinate frame which is "rotated" with respect to the robot's base or the robot's map coordinate frame. The "rotation" (pre-processing) and scaling (to find x-y from long-lat), are typically not included in the Jacobian.

In your example, if "rotating" the velocity sensor meant that it detected: $v+b$ for velcoity vector $v$ and some fixed bias $b$, then yes, simply removing the bias before incorporating the data is fine. Your innovation has the same value, since $h(x)$ is already the operation you described (applying the rotation).

But if the velocity sensor now measured $v+f(x)$, as in it measured greater velocity if the robot was turning right than left (as is the case of optical flow calculations from a camera facing a little to the right instead of directly forward), then you'd have to be much more careful. (Update jacobians or have near-perfect pre-processing).