Odometry

[ArthurAllshire]

We currently have no way to track how our chassis has moved on the field via odometry. This is a critical feature for automation during both the autonomous and teleoperated periods. It's not trivial to do accurately on a swerve drive (because due to wheel scrubbing, there is no solution to equations based off wheel odometry), so we need to figure out how we want to do this.

[james-ward]

One option might be to do dead reckoning using a Kalman filter. We could run it on each module by fusing the module azimuth, encoder readings and IMU. Then work out some way to combine the four modules. Or we fuse them all at the same time, which means we can attempt to eliminate bad readings.

[ArthurAllshire]

If we did something like that, what do you think the module filter would be doing? i.e what would the state space be and what would the predict / update steps look like?

[james-ward]

I've changed my mind - I think it's better to do the fusion of all modules together. That way we can more easily reject bad inputs.

[ArthurAllshire]

The method outlined in this comment may not be a bad starting point, and shouldn't be that hard to implement. I think it would just involve solving Ax = b with x as the robot's velocity vector, and b as the vector of x and y components of module velocities, as measured by encoders (i.e. [vx_a, vy_a, ... vx_d, vy_d]), right?

[Also, if we wanted to use a KF later on, I think A could act as our observation matrix as well]

[james-ward]

I wonder if we can do better gating beforehand though. For example, we know the vehicle vz component should be the same for each wheel, and it should be the same as measured by the IMU. So we could ignore any wheels that don't agree with this. Then we solve the least squares (or however we do it).

[ArthurAllshire]

You mean comparing from the robot's derived movement from module odometry to the expected movement of the robot over a timestep (from IMU / control inputs), and rejecting based on the residual?

[james-ward]

Yes, something like that. Should work pretty well I would think.

[ArthurAllshire]

Or perhaps a better solution could work like calculating the odometry across all the wheels (least squares / whatever), look for outlier modules (from both the the control input estimate and the odometry estimate), reject (or weight?) modules based off the difference, then feed the accepted modules back to our module-> chassis movement calculator.