I was looking into different ways of doing sensor fusion so it would be possible to get a more accurate robot pose and came across kalman filters. I understand that for autonomous, encoders are usually enough but I wanted to be able to get a somewhat accurate pose during teleop as well. It would be very much appreciated if anyone could give any help or advice.

Setting up a KF for localization this year was one of my goals. Itâ€™s still not done and probably wonâ€™t be for a while due to COVID-19, but Iâ€™ll be glad to tell you the approach Iâ€™ve taken!

If you just found out they exist, then thereâ€™s a lot of theory to learn first about state-space systems and estimation. Thereâ€™s plenty of free resources in the interwebs to learn about them. While you could just know what each variable means, take measurements, and translate it into code, it is very useful to understand why it works (especially when something breaks, so you can fix it). A while ago I put some links about KFs in this thread, so you can check those out.

Once you know how they work, the first step is to model your system, in this case itâ€™s probably going to be just the robot drive base. So you consider what states to use for the robot. Iâ€™m using x, y, yaw, left wheel velocities, right wheel velocities. You then have to consider things like what sensors will it have (encoders, vision (basically a must, or else it canâ€™t correct for errors) maybe IMU) and how to calculate their measurements given the robotâ€™s state.

A drive base is inherently nonlinear. The â€śnormalâ€ť Kalman filter gives you the best result when the system is perfectly linear, so you have to think about how to deal with nonlinearities. I started off by using an Extended Kalman Filter that linearizes the system at every time step, and then switched to an Unscented Kalman Filter because itâ€™s usually better at dealing with nonlinearities and it doesnâ€™t require calculating Jacobians which can get messy.

Then, you need to get real measurements from your robot to plug into the systemâ€™s model, like motor or characterization constants. Luckily WPILibâ€™s characterization tool outputs a raw JSON file that you can analyze with other programs determine how much noise the drive base has while moving, which is needed for the KF.

While itâ€™s not necessary, I highly recommend simulating the KF. Thereâ€™s a lot of places it could go wrong. I am very lucky that both my school and robotics team provide MATLAB licenses, which has made it a much smoother process since you can write and debug code without breaking a robot (also helps if you donâ€™t have physical access to your robot right now).

Then after that, you may have to tune it. For an EKF, this usually looks like changing the update rate until it gives acceptable performance. For a UKF, thereâ€™s some tuning parameters you can slightly tweak.

Finally, you can translate your simulation code (if applicable) into your robotâ€™s language, which involves installing libraries to deal with math. Iâ€™m using EJML for linear algebra and the Apache math library solving the differential equation that governs the robotâ€™s motion. If youâ€™re a C++ team, Iâ€™ve heard Jeigen is a good library for linear algebra.

Thereâ€™s a very high barrier to entry to get into advanced control and estimation, but once you overcome it theoretically you can keep doing it for years to come. Thereâ€™s currently a group of people working on adding a state space library to WPILib so hopefully that barrier will be much smaller next year

Hereâ€™s some threads on Chief Delphi with more info:

Thereâ€™s been a lot of threads recently about advanced control and estimation. Iâ€™m glad more people are getting into it now that we all have some free time! Itâ€™s a very interesting field but is sadly so technical that few teams can get into it, but thatâ€™s no reason to get discouraged!

Oops, almost forgot: some students at WPI published a paper about localization for FRC. I found it was a useful read to understand what options are available, but I took a different approach (different system model and using a UKF instead of EKF). It also has a section defining what a successful localization accomplishes, which is definitely worth knowing since your error will never be 0 so itâ€™s good to know whatâ€™s â€śacceptableâ€ť

Thank you for all the help and references! Because pretty much everything has been canceled, I have been trying to learn more about programming and control theory so we can implement it next year and I didnâ€™t really know where to start, and this really helps with that!

Note that the C++ library is Eigen and Jeigen is a Java wrapper for the C++ Eigen library. The upcoming WPILib state-space implementations use Eigen for C++ and EJML for Java.

I wrote an Extended Kalman Filter that we used on our robot this year to fuse vision, visual SLAM, and encoder measurements. The code is here. Note that some tuning issues prevented us from fusing everything â€śwellâ€ť during our first competition.

Here are some charts:

That EKF will be in wpilib next year. Iâ€™m also in the process of writing (along with Prateek whoâ€™s doing the C++ versions) easy to use latency-compensated wrappers that fuse vision pose measurements and encoders to allow for full field localization. These will also be in WPILib next year.

Thank you for the clarification, and unrelated but thank you for your tutorial on trajectory following, it was really helpful for us this year!

oh oops my bad! Thanks for correcting

Thank you for the reference! One question: What did you use for vSlam?

I wrote a custom Java wrapper for the Intel RealSense T265 camera that uses a version of librealsense which I patched to use less memory so it can run on the roboRIO. This is nice because it doesnâ€™t require any coprocessor. I intend to release this as a vendordep (along with a better-explained code release), but my WPILib work has gotten in the way (itâ€™s a higher priority for me.)

That sounds really interesting, is there a specific reason you chose the T265?

Oh I just realized I linked to your own topic! I knew that username sounded familiar lol

Haha yeah, since I have nothing to do all day Iâ€™ve just been looking into control theory applications in FRC so Iâ€™ve been on CD way too much.

yeah I think we all have tbh

I evaluated ORB-SLAM-2 and rtabmap, and they had similar or worse accuracy characteristics and both required a coprocessor.

If you have more questions then the FRC Discordâ€™s #programming-discussion channel is a great place for more â€śinteractiveâ€ť discussions. Tyler (who wrote the Controls Engineering in FRC book) also hangs out there and is always there to help with all my terribly misguided questions . He might be willing to help you too.

With the rate at which the error covariance increases for a drivetrain model, we didnâ€™t notice a difference between an EKF and UKF in sim. In fact, the UKF had a bit more noise in the state estimate. This may have been due to the UKF impl not discretizing Q and R based on the previously measured sample period like the EKF impl does.

Jacobians donâ€™t get messy if you compute them numerically. Thatâ€™s what my team did last year. Although, the analytical Jacobians of a 5-state drivetrain model arenâ€™t that bad anyway. The screenshot below is with respect to x and u, to compute A and B respectively. (States are x, y, heading, left velocity, and right velocity. Inputs are left and right voltage.)

Linearizing the state dynamics is just fine, the problem was with the outputs! The LimeLightâ€™s tx and ty have arctangents and distances in the calculations so I kept messing up the partial derivatives until I decided to use a UKF and not even deal with that lol. It didnâ€™t occur to me to numerically calculate the Jacobians, it might be worth pursuing. But I do like that the UKF is slightly more thorough, so Iâ€™ll keep it for now.

Speaking of, why does R change with the sample time? I never understood that - itâ€™s the same sensors and it feels like noise is inherent to the sensor

Hereâ€™s a numerical Jacobian implementation in EJML if you need it: https://github.com/mcm001/allwpilib/blob/state-space-v2/wpilibj/src/main/java/edu/wpi/first/wpilibj/system/NumericalJacobian.java

I havenâ€™t measured the resource utilization of an EKF vs UKF implementation on the roboRIO yet. Itâ€™s a balance between the extra computation from the unscented transform and the numerical Jacobians required for an EKF. Their APIs are almost drop-in replacements for each other.

I thought so too originally. Why would the noise contribution change if itâ€™s the same amount of noise being added per sensor update? The difference is that the Q and R weâ€™re providing is continuous, not discrete, so the conversion is complicated. Iâ€™m just going to lift this from chapter 6 of https://tavsys.net/controls-in-frc for now, because I need to go to bed. Iâ€™m aware it skips over requisite concepts like â€śexpected valueâ€ť and â€śdelta functionsâ€ť, so I can explain that stuff later if youâ€™re interested. If all you need is the formulas, hereâ€™s the overview. The discrete model is defined as follows:

\mathbf{x}_{k+1} = \mathbf{A}_d \mathbf{x}_k + \mathbf{B}_d \mathbf{u}_k + \mathbf{w}_k

\mathbf{y}_k = \mathbf{C}_d \mathbf{x}_k + \mathbf{D}_d \mathbf{u}_k + \mathbf{v}_k

where \mathbf{w} is the process noise, \mathbf{v} is the measurement noise, and both are zero-mean white noise sources with covariances of \mathbf{Q}_c and \mathbf{R}_c respectively. \mathbf{w} and \mathbf{v} are defined as normally distributed random variables.

\mathbf{w} \sim N(0, \mathbf{Q}_c)

\mathbf{v} \sim N(0, \mathbf{R}_c)

\mathbf{A}_d = e^{\mathbf{A}_c T}

\mathbf{B}_d = \int_0^T e^{\mathbf{A}_c \tau} d\tau \mathbf{B}_c = \mathbf{A}_c^{-1} (\mathbf{A}_d - \mathbf{I}) \mathbf{B}_c

\mathbf{C}_d = \mathbf{C}_c

\mathbf{D}_d = \mathbf{D}_c

\mathbf{Q}_d = \int_{\tau = 0}^{T} e^{\mathbf{A}_c\tau} \mathbf{Q}_c e^{\mathbf{A}_c^T\tau} d\tau

\mathbf{R}_d = \frac{1}{T}\mathbf{R}_c

To see why \mathbf{R}_c is being divided by T, consider the discrete white noise sequence \mathbf{v}_k and the (non-physically realizable) continuous white noise process \mathbf{v}. Whereas \mathbf{R}_{d,k} = E[\mathbf{v}_k \mathbf{v}_k^T] is a covariance matrix, \mathbf{R}_c(t) defined by E[\mathbf{v}(t) \mathbf{v}^T(\tau)] = \mathbf{R}_c(t)\delta(t - \tau) is a spectral density matrix (the Dirac function \delta(t - \tau) has units of 1/\text{sec}). The covariance matrix \mathbf{R}_c(t)\delta(t - \tau) has infinite-valued elements. The discrete white noise sequence can be made to approximate the continuous white noise process by shrinking the pulse lengths (T) and increasing their amplitude, such that \mathbf{R}_d \rightarrow \frac{1}{T}\mathbf{R}_c.

That is, in the limit as T \rightarrow 0, the discrete noise sequence tends to one of infinite-valued pulses of zero duration such that the area under the â€śimpulseâ€ť autocorrelation function is \mathbf{R}_d T. This is equal to the area \mathbf{R}_c under the continuous white noise impulse autocorrelation function.