Search code examples
pythonkalman-filter

Tracking a robot in circular motion using Kalman Filter

What I'm trying to do:

  • Localize the robot moving in a circular motion using Kalman Filter or Extended Kalman Filter
  • Using trigonometry and linear algebra, I am able to predict a "circular motion," but I wanted to find out if I can use the Kalman Filter to localize the robot (without assuming it's in the circular motion)
  • The robot senses its coordinate (x, y).

What I'm having a trouble with:

  • The state vectors from Kalman Filter converges to the center of the circle
  • The Kalman Filter fails to find the true positions
  • Screenshot: Robot vs Kalman Filter

My code Implementation

Solution

  • I think there are two issues here. One is that you are missing the process covariance matrix Q. If your state transition model is not perfect this will give the algorithm a hint of how uncertain the prediction is. A large Q will make the algorithm rely more on the measurements. Try initialising

    self.q = 0.001*self.f.dot(self.f.transpose())

    and later in the predict function

    self.p = self.f.dot(self.p).dot(self.f.transpose()) + self.q

    The other issue is that you are measuring a circular (polar) movement in an cartesian plane. The rotation gives an acceleration in X and Y and it is missing in the F matrix. I would update the F matrix to include the complete physical model including acceleration. The time step (dT) is also missing and could be added as an argument.

    class KalmanFilter(Filter):
    def __init__(self, sigma, dT):
    ...
    self.f = np.array([[1, 0, dT, 0, dT*dT/2,   0],
                       [0, 1, 0, dT,   0, dT*dT/2],
                       [0, 0, 1, 0,   dT,   0],
                       [0, 0, 0, 1,   0,   dT],
                       [0, 0, 0, 0,   1,   0],
                       [0, 0, 0, 0,   0,   1]])

    and finally in your main function

    KF = KalmanFilter(sigma=1,dT=0.1)

    I also increased sigma to 1 to get a smoother prediction and decreased the P initialisation to 1 from 999 to visualize the initial overshoot.

    Here is the result: enter image description here