Kalman filter for estimating position with “direction” measurements

Question

I am currently working on a pose estimation problem for which I would like to use filtering. To explain the system briefly, it consists of two cameras and each has its own GPS/IMU module. The main assumption is that Camera1 is fixed and stable, whereas camera2 has a noisy pose in 3D. I am using computer vision to obtain the pose (metric translation and rotation) of camera2 w.r.t. camera1, so that I can improve upon the inherent noise of GPS/IMU modules.

The problem here is that the translation obtained through the vision method is only up to an arbitrary scale, i.e. at any given instant, I can only obtain a unit vector that specifies the "direction" of the translation and not absolute metric translation. The camera based estimation, although accurate, has no idea about how much actual distance is between the cameras, which is why I have the GPS, which gives me position data with some noise.

Example: camera 2 is 5 m to the east of camera 1, the pose from my vision algorithm would say [1, 0, 0] ; 1 m north-east to camera 1, it would be something like [0.7, 0.7, 0]

Hence, would it be possible to consider the GPS estimate of the metric translation as well as its covariance ellipse, and somehow link it with the normalized camera measurements to obtain a final, more accurate estimate of metric translation? I am not sure what kind of filters would be happy to use a measurement that has no absolute value in it.

Thanks!

Answer 1

First, a point of terminology - I believe when you say "direction" what you mean is what is commonly called a heading. That is, when you say "direction" what you mean is the angle between an arbitrary line from camera 1 (usually straight forward) and the line between camera 1 and camera 2.

Assume camera 2 starts at a known distance from camera 1 (sufficiently long for a good filtered GPS location, hand placed, etc.) and assume you know the initial heading.

Call the initial distance between camera 1 and camera 2 $a$.

Now camera 2 moves. Your IMU reports the distance traveled. Call this distance $b$.

Camera 1 reports the change in heading between the initial and second location, this is an angle (from the arc tangent of your camera 1 output). Call this angle $B$.

Now you have an $a$, a $b$, and a $B$. Use the Law of Sines to calculate the distance between camera 1 and camera 2 - this will be $c$.

You can now use a basic Kalman filter to filter the IMU distance output to get a better $b$, and thus a better $c$, and you can use another to combine this reading with your noisy GPS position. The method I've described should give you better short term results than the GPS, and the GPS should correct any drift caused by long term error accumulation.