Kalman filtering implementation

[matthewashton-k]

Issue to track the progress of integrating kalman filtering to the program.

[Hydro111]

Progress is being made in kalman-filter-integration. The filter library can be seen in utahrobotics/kalman-filter

[Hydro111]

Next steps:

• Verify simple code with rerun on full repo (logs are currently not being recognized, among other problems)
• Rewrite the localizer task

[Hydro111]

Definition of Done

Localization works without large error on real data, using kalman filtering, with or without proper variance estimates.

[Hydro111]

System has been researched, here are some preliminary notes on what needs to be done, and how it could be done:

localizer.rs doesn't seem to do much complicated processing. Most fusion simply consists of taking parts of some sensors and splicing them with data from other sensors. It's trivial to recreate this with a kalman filter.

Notes on each data source:

• IMU - Easy to implement. This just requires the IMU to send over some variances (likely constant) along with its current information.
• KISS ICP - Some odd stuff going on here. Because this compares current to past point clouds, it's relative rather than absolute. The loop in icp_pipeline.rs should likely take in the localizer position for comparison instead of using its own past value. Variances might be able to be calculated based on the data, but this would require someone more familiar with the system. This system just provides position and orientation.
• PICO V3 - What is even going on here? It would seem this is supposed to encapsulate four IMUs and the bucket actuator, but it also seems to just be stubbed in the code right now. This is also completely unused in the current localizer. Probably best to ignore this for now.
• Apriltags - Simple, but nontrivial. Tag measurements need to be packaged with their uncertainty, ideally in proper covariance matrix form. Ideally, this would actually simplify the apriltag logic, as the Kalman filter can perform intersections and reject awful measurements. It's possible that the covariance matrices could just be generated by scaling and rotating a base matrix.
• Encoders - Missing! Guess we should wait until they're actually on the robot for that? The motors are brushless iirc, so we very well might just be able to read their encoders over CAN already.

The kalman filter will likely use a 15 vector. It's components (in no particular order) are currently planned to be the following:

• 3 Position
• 3 Velocity
• 3 Acceleration
• Orientation (3 vec for axis angle representation)
• Angular velocity (3 vec for axis angle representation)
  The angular components aren't totally ideal, but they seem to be fairly good. The key is that they can be linearly interpolated in a fairly reasonable manner, and there are no invalid constructions of the variables (i.e. a one-to-one mapping from 3 vectors to rotations). These properties mainly need to remain true locally around any value, but global would be better (and might be impossible). If any sensor produces angular acceleration, then that should be added.

[Hydro111]

Analysis of using axis angle representation for things: https://www.desmos.com/3d/3jg73klxpu

[Hydro111]

Discussion with Russell led to the following plans:

• Communication to the localizer task will follow the following format:
  Struct with a name "Measurement", which contains...
  • Arrays of f64s corresponding to each measured, using the following names, in the following order (not all should be used):
    • position
    • velocity
    • acceleration
    • orientation
    • angular_velocity
  • A 1D array of f64s containing the elements of the combined covariance matrix for all values provided

Tasks will be divided as follows:

• Hale - localizer.rs, april tags, and structural changes
• Russell - IMUs and ICP updates

[Hydro111]

localizer.rs is nominally complete, as of 22a760d. Changes will likely be needed as data inputs are updated.