Principle Introduction and Theoretical Support Analysis: Linear Kalman Filters
We keep track of our own odometry and tracked targets using simple linear kalman filters. All our kalman filters use a constant acceleration model.
We believe DJI has published a comprehensive article on the theory of linear kalman filters on the RoboMaster forums before so we will only be giving a brief introduction theory-wise.
There were multiple problems that we were unable to resolve this season so this page will be dedicated more to what we got wrong and the possible areas of improvement.
We will give a very brief and non-mathematically rigorous introduction of the Kalman filter.
Standard Kalman Filter notation:
- state vector, x, at timestep k
- transition matrix, F, at timestep, k
- control matrix, B, at timestep, k (we do not use this)
- control vector, u, at timestep k (we do not use this)
- measurement matrix, H, at timestep, k
- measurements, z, at timestep k
- process noise covariance, Q, at timestep k
- measurement noise covariance, R, at timestep k
- Kalman gain, K, at timestep k
- error covariances, P, at timestep k
@ denotes matrix multiplication
We will use d to denote the number of state parameters and m to denote the number of measurement parameters.
We use OpenCV's implementation of the Kalman Filter.
A Simple linear Kalman filter estimates the state at each timestep, , by the following process. It first makes an estimation of the current state using the transition model: predicted state = transition matrix @ previous state vector + control matrix @ control vector. This is typically called the 'predict step'. We denote this predicted state at timestep k as
:
We don't use the control matrix and control vector so this simplifies to
for us.
After obtaining , the filter obtains a more accurate reading by correcting it using the measurements acquired on this timestep. This is typically called the 'update step'. First it obtains what the measurements should be on this timestep based on the predicted
: predicted measurements = measurement matrix @ predicted state. We denote this predicted measurement at timestep k as
.
It then calculates, , the difference between the actual measurement,
, and the predicted measurements:
. After that, it calculates the Kalman gain,
, using the process noise covariance,
, the measurement noise covariance,
, and the predicted error covariances,
(calculation for
not shown). It then uses the Kalman gain,
, to update the predicted state to get the optimal state estimate for this timestep:
The Kalman gain is then also used to give the optimal estimates for the error covariances, .
We've skipped presenting all the maths relating to the calculating of covariance estimates because their derivation and presentation are pretty lengthy, and RoboMaster has published an article on this before.
However, intuitively, the Kalman gain, , is a d x m matrix that maps from measurement space into state space, and it gives a measure of how much the filter trusts the measurement,
, over the prediction,
. Kalman gain increases with a larger process noise,
, and decreases with a larger measurement noise,
. At the extreme where
is a zero matrix, the update step above becomes
and the measurement is completely discarded. On the other extreme,
converges to the pseudo inverse of
, and the new state is just based on the measurement.
While the linear Kalman filter is optimal for linear processes, it is obvious that robot movement is a non-linear process. Since our constant acceleration model for robot movement is wrong, error covariances, , constantly increase in magnitude, and eventually our Kalman filter diverges.
We've made a temporary workaround by resetting the Kalman filters when the error covariances get too large. However, this is clearly not ideal.
As such, we will be exploring other state tracking and filtering algorithms more suited to our use case, such as the Extended Kalman Filter, Unscented Kalman Filter, Particle Filter, or even just moving averages, this next season.
We also did not spend enough time tuning the noise covariances (process noise and measurement noise
) of our Kalman Filters.
There were other problems and mistakes that we made in our implementation of the linear Kalman filters as well, and we will detail them in the individual sections below.
We hope that other teams can study these mistakes and build upon them.
All our measurements use the ROS coordinate system (+x forward, +y left, +z up) and radians for angular values. For angular values, positive is always counter-clockwise, i.e. the direction of your finger grip when pointing your right thumb in the direction of the axis.
Four wheel odometry keeps track of the odometry data of all four wheel mecanum drive robots using data from wheel motor encoders and the IMU onboard the MCB Type A. We primarily referred to this paper for the math to calculate the robot's odometry. In particular, we used equation (2) and fig. 3 on the pages labelled 589 and 590 (page 3 and 4 on the PDF linked).
aruw_odometry/src/four_wheel_mecanum_drive.py
We use 2 Kalman filters. One Kalman filter is in the chassis frame of reference while the other Kalman filter is in the IMU frame of reference.
Per common notation, v for velocity, a for acceleration, for angular velocity and
for angular acceleration
Chassis Kalman Filter
State vector :
Transition Matrix :
Measurements :
Measurements are calculated from the wheel RPMs using the formula detailed in the paper reference above.
Measurement Matrix :
Process and measurement noise covariances were not well-tuned.
IMU Kalman Filter:
State vector :
Transition Matrix :
Measurements :
Measurements are raw measurements from the IMU onboard the MCB Type A, except , which is obtained from the state estimate of the chassis kalman filter. The purpose of
is to double as a measurement for
to mitigate yaw drift since we do not use the magnetometer to do so.
Measurement Matrix :
Process and measurement noise covariances were not well-tuned.
Like four wheel odometry, sentry odometry keeps track of the odometry data of the sentry robot using data from wheel motor encoders and the IMU onboard the MCB Type A. The calculations were simpler since movement of the robot is confined to a single plane.
aruw_odometry/src/sentinel_drive.py
Again, we use 2 Kalman filters. One Kalman filter is in the chassis frame of reference while the other Kalman filter is in the IMU frame of reference.
Chassis Kalman Filter
State vector :
Transition Matrix :
Measurements :
Measurements are calculated by averaging the wheel RPMs and then using yaw readings from the IMU to determine direction.
Measurement Matrix :
Process and measurement noise covariances were not well-tuned.
IMU Kalman Filter:
State vector :
Transition Matrix :
Measurements :
Measurements are raw measurements from the IMU onboard the MCB Type A
Measurement Matrix :
Process and measurement noise covariances were not well-tuned.
-
Using 2 separate Kalman filters to keep track of odometry was a big mistake. We should be combining the measurements into a single Kalman filter and let the filter do the relative calculations instead of manually doing the relative calculations. For example, we currently use the four wheel odometry's yaw predictions from the chassis kalman filter to calculate relative vx and vy. To do this, one possible option is an Extended Kalman Filter which would require us to define the function that maps all the current odometry state variables - linear and angular position, velocity and acceleration - to the next predicted state. In the case above, if we had used an Extended Kalman Filter, the state transition mathematical function would simply capture how the current state's yaw affects the predicted vx.
-
Experiment with variable process and measurement covariances (
and
) that vary with
-
Consider adding in user commands as a control vector for greater accuracy. However, these commands will likely have to be filtered due to latency.
We track our targets across time using a simple linear kalman filter with a constant acceleration model adjusted for time deltas as well.
For each tracked target, we match its current state measurement to the detected target that is closest to its predicted position. For more information, see: https://github.com/uw-advanced-robotics/aruw-vision-platform-2019/wiki/Principle-Introduction-and-Theoretical-Support-Analysis:-Target-Tracking-and-Target-Selection
aruw_odometry/src/tracked_target.py
All variables are in the static ('odom') frame. Per common notation, v for velocity, a for acceleration.
State vector :
Transition Matrix :
Measurements :
Measurements are calculated by converting 3-D readings from the camera frame into the static frame.
Measurement Matrix :
Process and measurement noise covariances were not well-tuned.
- As was mentioned above, it seems like the linear kalman filter is not good enough.
- Consider using velocity and acceleration data from our own odometry as a control vector