I am currently in the process of writing a pose estimation algorithm using image data. I receive images at 30 fps, and for every image, my program computes the x,y,z and roll, pitch, yaw of the camera with respect to a certain origin. This is by no means very accurate, there are obvious problems such as too much exposure in the image, not enough feature points in the image, etc., and the positions go haywire every once in a while; so I want to write a Kalman filter that can take care of this part.
I have read through the basics of KF, EKF etc. and then I was reading through an OpenCV tutorial that has an implementation of a Kalman Filter inside an algorithm for the pose estimation of an object. While this matches my use case very well, I don't understand why they are using a linear Kalman Filter while explicitly specifying parameters like (dt*dt) in the state transition matrix. For reference, the state transition matrix they are considering is
/* DYNAMIC MODEL */
// [1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0 0 0]
// [0 1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0 0]
// [0 0 1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0]
// [0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2 0 0]
// [0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2 0]
// [0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2]
/ [0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1]
I'm a little confused, so my main question can be broken down into three parts:
- Would a linear Kalman Filter suffice for a 6DOF pose estimation filtering? Or should I go for an EKF?
- How do I come up with the "model" of the system? The camera is not really obeying any trajectory, the whole point of the pose estimation is to track the position and rotation even through noisy movements. I don't understand how they came up with that matrix.
- Can the Kalman Filter understand that, for instance, if the pose estimation says my camera has moved half a meter between one frame and other, that's plain wrong, because at 1/30th of a second, there's no way that could happen?
Thank you!