SwerveDrivePoseEstimator with non-zero "A"

[truher]

Hi, I guess this is mostly a question for @calcmogul. Team 100 is using the SwerveDrivePoseEstimator, and kinda wishing that the embodied linear system had an "A" term. I dimly recall that the version of SDPE from 2022 used an EKF that did have "A", but the EKF was removed in December. Am I remembering correctly? The commit message from that time said that the reason to change was simplicity, but I'm wondering if there were any other reasons, like this would be a bad path for me to direct my control engineering students down. They're quite interested in state space controls, and also in very precise swerve driving, so maybe it would be a good way to get started? What do you think?

[calcmogul]

I dimly recall that the version of SDPE from 2022 used an EKF that did have "A", but the EKF was removed in December.

The 2022 pose estimator used the following model with a UKF:

$$
\begin{align*}
\frac{dx}{dt} &= \mathbf{A}\mathbf{x} + \mathbf{B}\mathbf{u} \
\mathbf{y} &= \mathbf{C}\mathbf{x} + \mathbf{D}\mathbf{u} \
\frac{dx}{dt} &= \mathbf{0}{3x3}
\begin{bmatrix}
\text{x position} \
\text{y position} \
\text{heading}
\end{bmatrix} + \mathbf{I}{3x3}
\begin{bmatrix}
\text{x velocity} \
\text{y velocity} \
\text{angular rate} \
\end{bmatrix} \
\mathbf{y} &=
\begin{bmatrix}
0 & 0 & 1
\end{bmatrix}
\begin{bmatrix}
\text{x position} \
\text{y position} \
\text{heading}
\end{bmatrix} + \mathbf{0}_{1x3}
\begin{bmatrix}
\text{x velocity} \
\text{y velocity} \
\text{angular rate} \
\end{bmatrix}
\end{align*}
$$

where the input came from swerve drive velocity kinematics.

We noticed that the UKF had occasional stability issues (usually when the reconstructed error covariance matrix had poor condition number) and the measurement replay during vision updates was very slow. A UKF does the same prediction and correction with multiple sigma points to approximate the true nonlinear probability distribution, so the filter effectively gets run 2k+1 times per timestep where k is the number of states; it's like a particle filter but more intelligent with how it selects the samples.

Since the dynamics and measurement model were both linear, the UKF behavior was equivalent to just using a linear Kalman filter. Furthermore, since the model actually had no dynamics (the input and pose measurements drove the model directly), we could simplify further to just a weighted average between the SwerveDriveOdometry pose estimate and the vision pose estimate.

We still use an optimal steady-state Kalman gain, but given the simple model we have, the continuous algebraic Riccati equation simplifies to this: https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/algorithms.md#closed-form-kalman-gain-for-continuous-kalman-filter-with-a--0-and-c--i

I agree that it's kind of disappointing, but the fancy UKF we used really just simplifies to this. Our original model was a gross simplification of the true nonlinear state estimation problem (for one thing, the pose measurement distributions aren't Gaussian). Given that we have SE(3) poses coming in at different timestamps, I've been told would be more accurate to model it with factor graphs.

We've attempted to model swerve dynamics before, but it ended up not being accurate: #3374. The prediction method we're using now is essentially swerve drive position delta kinematics

[truher]

thanks! I was imagining maybe something simple, like a one-dimensional KF for each axis, to account for the basic physics. does that seem reasonable?

[calcmogul]

What we're currently doing is a 1D KF along each axis. What dynamics did you have in mind specifically?

[truher]

well, momentum, I guess. it seems like the estimator could let the system evolve without any new observations, and that would be useful as a kind of smoothing.

[calcmogul]

Given how heavily weighted Kalman filters are toward encoder/gyroscope measurements in FRC, I personally don't see how a dynamics model (e.g., position/velocity states, voltage inputs) would improve the state estimate. You should be feeding measurements in every timestep (at least as often as 20 ms), and the encoder sensor noise is tiny relative to the model inaccuracy (process noise), so the optimal contributions determined by the Kalman gain mean that the encoders and gyro are trusted completely over the model. The encoder position and gyro heading being the model drivers is essentially what SwerveDriveOdometry does with the SE(2) exponential map.

3512 had a differential drive EKF in 2020, and we also saw experimentally that the odometry class gave the same results as the Kalman filter.

[truher]

ok, thanks, makes sense. i think our situation is a little bit unusual because we drive our "practice" swerve drivetrains very fast on very hard wheels so the odometry is not very accurate at all, whereas our camera system is very accurate. i think (perhaps because of the wheel situation) we also have not paid very much attention to sensor accuracy, so to us odometry seems like (and is) a source of noise instead of a source of certainty. in fact, i have in mind to do an off-season controls project that purposely saturates the wheels, kinda like the stanford drifting delorean, since we're almost there anyway... :-) . anyway thanks again.