Extended Kalman Filter

In my project, the goal is to implement an Extended Kalman Filter. I am estimating the state of a mobile robot; this state consists of the robot's 2D position and orientation (x, y, theta). The command to the robot includes a forward translational velocity, vel_trans, and a rotational velocity, vel_ang. Therefore, the dynamic model of the robot is as follows:

 

x(k+1) = x(k) + t * vel_trans * cos(theta),

y(k+1) = y(k) + t * vel_trans * sin(theta),

theta(k+1) = theta(k) + t * vel_ang,

 

where t is the length of the time step, which I assume to be 0.05 seconds.

 

At each time step, the robot will publish both vel_trans and vel_ang. However, it's important to keep in mind that the dynamic model is not perfectly accurate. I should assume that the error between the model output and the actual robot pose is zero-mean white noise. The covariance of this noise is 0.0025 for the translational components of the state and 0.005 for the rotational component.

​

In addition to publishing the commanded velocities, the robot can also localize itself with respect to a number of landmarks in the scene. Whenever a landmark is within the range of the robot's sensors, the robot will record its distance to the landmark, as well as the bearing of the landmark relative to the robot's orientation. Assuming the robot is at coordinates (xr, yr, thetar) and the landmark is at coordinates (xl, yl), the range and bearing are computed as follows:

 

range = math.sqrt((xr - xl) * (xr - xl) + (yr - yl) * (yr - yl)),

bearing = math.atan2(yl - yr, xl - xr) - thetar.

 

The robot will publish a range and bearing for each of the landmarks it can see at a given time, along with its velocity commands. Due to sensor noise, these measurements will not be perfectly accurate. I should assume zero-mean white noise with a covariance of 0.1 for the range and 0.05 for the bearing measurement.