50.2.1 Process noise Q and W (odometry)

See also: Factors affecting Kalman filter performance

Source: Tereshkov 2015

• Process noise covariance matrix has no clear physical meaning, cannot be deduced from sensor characteristics
• Leads to non-intuitive, iterative procedures to tune KFs
• Which means that KF optimality is rarely achieved in practice

Alternative to KF tuning: the use of geometric observers

• estimates are expresssed only in terms of quantities with clear geometrical meaning

Source: Schneider 2013

• If perfect model: $Q$ only describes the covariance of the random process noise
• Not perfect model, has:
  • parametric errors (-> parameter identification)
  • structural erors (error in model structure)
  • workaround: e.g. choosing from a set of candidate models, etc.
• If model parameters are to be estimated: (param_est, Cov_param_est)
  • the covariance of the estimated parameter is often ignored in EKF design

 ( if representing the stochastic part as an additive component)

To calculate $Q$ from $C_p$

1. Set $$ \mathbf{p} = \hat{\mathbf{p}} + \delta \mathbf{p}$$ 

2. Do Taylor series expansion on prediction equation $$ \dot{\mathbf{x}}(t) = \mathbf{f} \left( \mathbf{x}(t), \mathbf{u}(t), \mathbf{\hat{p}} \right) + \mathbf{J}{\hat{\mathbf{p}}}(t) \delta \mathbf{p} $$ $$ \mathbf{J}{\hat{\mathbf{p}}}(t) = \left( \dfrac{\delta \mathbf{f}}{\delta \mathbf{p}} \right) _{\mathbf{x}(t), \mathbf{u}(t), \mathbf{\hat{p}} }$$

3. Calculate $Q$

$k_Q \geq 1$ (make larger if it plant-model seems very mismatched)

• $C_p$ is time invariant
• $Q$ is time-varying as J is time-varying

Source: rlabbe

What does process noise mean?

Deviation of real system from deterministic system, e.g. due to unforeseen disturbances (human intervention, weather, wind, etc) $$ \dot{\mathbf{x}} = f(\mathbf{x}) + w $$

No noise is added to $\mathbf{x}$ because the noise is white (0 mean)

Formula for Q

$$ Q = \mathbb{E} \left[ \mathbf{w} \mathbf{w}^\text{T} \right] $$

• Important that there is a nonzero process noise or process variance, to avoid the filter becoming a  smug filter
• A small value for process noise tells the filter that the prediction is very trustworthy, so this will result in a filter estimate that is quite straight (i.e. unnoisy), even though the sensor measurements are themselves very noisy

• However, as the process variance indicates how much the system changes with time, if it is very low, the filter will be slow to react / won’t correctly respond to meaasurements
• In short: the filter requires the variance to correctly describe the system behaviour in order for it to perform well!

Source: SLAM for Dummies

Used in step 1 (odometry update/prediction step)

Process is assumed to have a gaussian noise proportional to the controls $\Delta x$, $\Delta y$, $\Delta t$.

Process noise $Q \in \mathbb{R}^{3\times 3}$ $$ \begin{aligned} Q &= c \cdot \left[ \begin{array}{ccc} \Delta x^2 & \cdots\
& \Delta y^2& \cdots\
&& \Delta t^2 \end{array} \right]\
&= c \cdot \left[ \begin{array}{rrr} \Delta x^2 & \Delta x \Delta y & \Delta x \Delta t\
\Delta y \Delta x & \Delta y^2 & \Delta y \Delta t\
\Delta t \Delta x & \Delta t \Delta y & \Delta t^2 \end{array} \right]\
\end{aligned} $$

$C$

• Gaussian sample
• Representation of how exact the odometry is
• Set according to the robot odometry performance
  • e.g. by experiments and then tuning the value

$W$

$$W = \left[\begin{array}{ccc} \Delta t \cos \theta & \Delta t \sin \theta & \Delta \theta \end{array}\right]^\text{T}$$

$$Q = WCW^\text{T}$$