ObserverKalmanFilter

ObserverKalmanFilter estimates a joint’s position, velocity and torque from noisy sensor readings, using a model of the joint to reject noise the sensor cannot distinguish from signal. Its purpose is torque estimation: turning a noisy torque reading into a smooth one you can control against.

Unlike Observer, you do not tune a gain. You tell the filter how much to trust the model and how much to trust the sensors, and it works the gain out for itself, every update. That is the right structure when the model is approximate and the sensors are noisy.

flowchart LR
    i1(["inputVector — commanded torque"]) --> B["ObserverKalmanFilter"]
    i2(["measurementVector — measured position, velocity, torque"]) --> B
    i3(["stateVectorInit"]) --> B
    i4(["disable"]) --> B
    B --> o1(["stateVector — estimated position, velocity, torque"])
    B --> o2(["covarianceMatrix — the filter's own uncertainty"])
    B --> o3(["isEnabled"])

The model is $I\ddot{\theta} + b\dot{\theta} + k\theta = \tau - F$, where the friction force $F$ is $c_v\dot{\theta} + c_c\operatorname{sign}(\dot{\theta})$ — a velocity-proportional part plus a constant-magnitude part. Tuning is the ratio of processNoiseCovariance to measurementNoiseCovariance: raise the first to follow the sensors, raise the second to trust the model and smooth harder. The third diagonal element of processNoiseCovariance is what decides how fast the torque estimate may move.

measurementVector needs all three values — position, velocity and torque. Leaving the velocity element at zero tells the filter the joint is measured to be stationary, which fights its own estimate.

Never set measurementNoiseCovariance to zero. It is the natural way to say “trust the sensors completely” and it makes the filter’s arithmetic ill-conditioned. Never set frequencyDivider to zero either — see Limits and errors.

Signals

Inputs

Outputs

Parameters

All nine parameters are persistent and survive a controller restart. The inputs and outputs do not. No parameters exist below this block other than the five model/ entries listed above.

Setup

1. Measure or estimate the joint’s inertia, damping and stiffness. These are physical properties, not tuning knobs — get them roughly right before touching the covariances.

2. Set model/inertia, model/damping and model/stiffness. model/inertia must be above zero.

3. Set the two friction coefficients from the joint’s own friction measurement, or leave them at their defaults if you have no figure.

4. Link inputVector from the commanded torque, and all three elements of measurementVector from the sensors.

5. Confirm all three measurement elements move. Trace each one. A stuck zero on the velocity element is the most damaging misconfiguration of this block, because the filter treats it as a measurement of standstill.

6. Leave frequencyDivider at 1, and both covariances at identity.

7. Set enable true and watch element 2 of stateVector against the raw torque measurement on one trace. The estimate should be a smoothed version of the measurement, following it without the noise.

   Step 7 is the only check that the model is right. If the estimate drifts away from the measurement rather than smoothing it, the model is wrong — go back to step 1. No covariance setting fixes a wrong model.

8. Raise frequencyDivider only if the block costs more processing time than you can afford. Its correction step is the expensive part.

Tuning

1. Get the model right first, per Setup step 7. The covariances trade noise against lag; they cannot correct a wrong inertia.
2. Tune the ratio, not the absolute values. Only the ratio of processNoiseCovariance to measurementNoiseCovariance affects the result, so leave one at identity and move the other.
3. Start by raising measurementNoiseCovariance — multiply the whole matrix by 10 — and watch element 2 of stateVector. It should get smoother and slower.
4. Stop when the lag becomes unacceptable for whatever consumes the estimate. Smoothness and lag are the whole trade here.
5. If you need the torque estimate to respond faster without touching the others, raise only the third diagonal element of processNoiseCovariance. That element governs the torque state alone.
6. Watch the diagonal of covarianceMatrix while you tune. It should settle to steady values. A diagonal that keeps growing means the filter is not getting useful corrections — check step 5 of Setup.
7. Tune empirically, from the traces. Do not compute the covariances from sensor datasheets and expect the theoretical result: this filter’s internal gain does not correspond exactly to the model it runs, so the numbers that work will not be the numbers theory predicts.
8. Re-check nothing after a task-rate change to the covariances themselves, but do re-check frequencyDivider — it counts cycles, so the correction rate in hertz moves with the task period.

Read the trade off any curve: smoother means slower.

A starting point for a joint with 0.5 kg·m² inertia on a 1 ms task, correcting every cycle, trusting the sensors and the model equally:

enable                          = true
frequencyDivider                = 1
model/inertia                   = 0.5
model/damping                   = 10.0
model/stiffness                 = 0.0
model/viscousFrictionCoefficient  = 0.10
model/coulombFrictionCoefficient_ = 0.10
processNoiseCovariance          = identity
measurementNoiseCovariance      = identity

Then work Tuning step 3. This is a starting point, not a final tuning.

Limits and errors

The block raises no errors or warnings and logs nothing. Every failure above shows as a value on a trace, not as a message.

Verified against motorcortex-control3 3.30.0 (bc348fd).