Kalman Filter to Correct Loop Closure

146 Views Asked by At

I need to know if a Kalman filter is a good solution to the scenario that follows:

I have a drone flying autonomously by wave point navigation without a GPS. The system has an image processing unit that I have designed that feeds its location to my flight controller based off of all translations and rotations detected. The flight controller is independent and the only communication occurring is me feeding it location data and it responds accordingly. When I fly in a loop my image processing algorithm can detect that at the end of the loop I am in the place I started. However due to the image processing error, I have fed the wrong location to the flight controller throughout my loop. Thus, the flight controller thinks I am in a different location than it actually is. I now need to update my current location to the flight controller. Typically, this is how a GPS updates a system using a normal Kalman filter. My issue is that my flight controller cannot handle a large change in position. Therefore I need to smoothly update my position over the time after loop closure so that I eventually am giving the correct location (obviously as time goes on I will accumulate more error). Would a Kalman filter be an effective tool to smoothly update my position so that over a reasonable amount of time I am feeding the correct location to my flight controller?

1

There are 1 best solutions below

0
On

From your description it appears that your system is not observable most of the time. A Kalman filter can only improve the position estimate if you can also measure the position. In order to ensure that the Kalman filter would still not drift over time you need that the amount of drift, while not back at the starting position, is less then the Kalman filter can correct, while it is back at the starting position; so you might have to hover near the starting position for a while to ensure this (you can only do prediction steps or only correct with the velocity measurement while not near the the starting position).

You probably will still have some jump in the position estimate with the Kalman filter, but it will be spread out over time a little. A Kalman filter is an optimal observer for a linear time invariant system subjected to zero mean white noise on the inputs and outputs. However, you can also pick different values for the noise co-variance matrices in order to ensure smaller jumps in the state estimate. To reduce this further you can also use a constant observer gain (also called a Luenberger observer) by using pole placement and place the poles to ensure that the error doesn't converges too fast to zero.

But maybe a better alternate would be to ensure that the system is observable all/most of the time. To do this you could maybe map the surrounding or use SLAM.