使用卡尔曼滤波器跟踪机器人的圆周运动

Question

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

• Gist Link

Answer 1

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: