mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
e22d8cc343
This effectively replaces the Unscented Kalman Filter used for Pose Estimation with the Odometry model, and uses a recalculable Kalman gain matrix to update pose estimations whenever a vision measurement is added. Notably, this change removes the need for the confusing generics used in Java, and the C++ implementation got quite a bit less complex as well. Co-authored-by: Tyler Veness <calcmogul@gmail.com>
89 lines
1.2 KiB
Markdown
89 lines
1.2 KiB
Markdown
# Algorithms
|
||
|
||
## Closed form Kalman gain for continuous Kalman filter with A = 0 and C = I
|
||
|
||
### Derivation
|
||
|
||
Model is
|
||
|
||
```
|
||
dx/dt = Ax + Bu
|
||
y = Cx + Du
|
||
```
|
||
|
||
where A = 0, B = 0, C = I, and D = 0.
|
||
|
||
The optimal cost-to-go is the P that satisfies
|
||
|
||
```
|
||
AᵀP + PA − PBR⁻¹BᵀP + Q = 0
|
||
```
|
||
|
||
Let A = Aᵀ and B = Cᵀ for state observers.
|
||
|
||
```
|
||
AP + PAᵀ − PCᵀR⁻¹CP + Q = 0
|
||
```
|
||
|
||
Let A = 0, C = I.
|
||
|
||
```
|
||
−PR⁻¹P + Q = 0
|
||
```
|
||
|
||
Solve for P. P, Q, and R are all diagonal, so this can be solved element-wise.
|
||
|
||
```
|
||
−pr⁻¹p + q = 0
|
||
−pr⁻¹p = −q
|
||
pr⁻¹p = q
|
||
p²r⁻¹ = q
|
||
p² = qr
|
||
p = sqrt(qr)
|
||
```
|
||
|
||
Now solve for the Kalman gain.
|
||
|
||
```
|
||
K = PCᵀ(CPCᵀ + R)⁻¹
|
||
K = P(P + R)⁻¹
|
||
k = p(p + r)⁻¹
|
||
k = sqrt(qr)(sqrt(qr) + r)⁻¹
|
||
k = sqrt(qr)/(sqrt(qr) + r)
|
||
```
|
||
|
||
Multiply by sqrt(q/r)/sqrt(q/r).
|
||
|
||
```
|
||
k = q/(q + r sqrt(q/r))
|
||
k = q/(q + sqrt(qr²/r))
|
||
k = q/(q + sqrt(qr))
|
||
```
|
||
|
||
### Corner cases
|
||
|
||
For q = 0 and r ≠ 0,
|
||
|
||
```
|
||
k = 0/(0 + sqrt(0))
|
||
k = 0/0
|
||
```
|
||
|
||
Apply L'Hôpital's rule to k with respect to q.
|
||
|
||
```
|
||
k = 1/(1 + r/(2sqrt(qr)))
|
||
k = 2sqrt(qr)/(2sqrt(qr) + r)
|
||
k = 2sqrt(0)/(2sqrt(0) + r)
|
||
k = 0/r
|
||
k = 0
|
||
```
|
||
|
||
For q ≠ 0 and r = 0,
|
||
|
||
```
|
||
k = q / (q + sqrt(0))
|
||
k = q / q
|
||
k = 1
|
||
```
|