Wobble smoothing

To reduce high frequency noise.

Algorithm: Wobble smoothing
Input : { ( x [ k ] , y [ k ] , t [ k ] ) ∈ ℝ 2 × ℝ + , 0 ≤ k ≤ n } \left\{ \left( x\lbrack k\rbrack,y\lbrack k\rbrack,t\lbrack k\rbrack \right) \in {\mathbb{R}}^{2} \times {\mathbb{R}}_{+},0 \leq k \leq n \right\} , Δ T > 0 \Delta T > 0 (from wobble_smoother_timeout), v min v_{\text{min}} (from wobble_smoother_speed_floor) and v max v_{\text{max}} ( from wobble_smoother_speed_ceiling)
Compute a weighted moving average of the positions p ¯ [ j ] = ( x ¯ [ j ] , y ¯ [ j ] ) \underset{¯}{p}\lbrack j\rbrack = \left( \underset{¯}{x}\lbrack j\rbrack,\underset{¯}{y}\lbrack j\rbrack \right) ∀ j ∈ ⟦ 0 , n ⟧ , p ¯ [ j ] = { ∑ k = 1 n p [ k ] ( t [ k ] − t [ k − 1 ] ) 𝟙 [ t [ j ] − Δ T , t [ j ] ] ( t [ k ] ) ∑ k = 1 n 𝟙 [ t [ j ] − Δ T , t [ j ] ] ( t [ k ] ) if the numerator ≠ 0 p [ j ] otherwise \forall j \in ⟦0,n⟧,\underset{¯}{p}\lbrack j\rbrack = \begin{cases} \frac{\sum_{k = 1}^{n}p\lbrack k\rbrack\left( t\lbrack k\rbrack - t\lbrack k - 1\rbrack \right)\mathbb{1}_{\left. \left\lbrack t\lbrack j\rbrack - \Delta T,t\lbrack j\rbrack \right\rbrack \right.}\left( t\lbrack k\rbrack \right)}{\sum_{k = 1}^{n}\mathbb{1}_{\left. \left\lbrack t\lbrack j\rbrack - \Delta T,t\lbrack j\rbrack \right\rbrack \right.}\left( t\lbrack k\rbrack \right)} & \text{if the numerator } \neq 0 \\ p\lbrack j\rbrack & \text{otherwise} \end{cases} Calculate a moving average velocity v ¯ [ j ] \underset{¯}{v}\lbrack j\rbrack ∀ j ∈ ⟦ 0 , n ⟧ , v ¯ [ j ] = { 0 j = 0 ∑ k = 1 n ∥ p [ k ] − p [ k − 1 ] ∥ 𝟙 [ t [ j ] − Δ T , t [ j ] ] ( t [ k ] ) ∑ k = 1 n ( t [ k ] − t [ k − 1 ] ) 𝟙 [ t [ j ] − Δ T , t [ j ] ] ( t [ k ] ) otherwise \forall j \in ⟦0,n⟧,\underset{¯}{v}\lbrack j\rbrack = \begin{cases} 0 & j = 0 \\ \frac{\sum_{k = 1}^{n}\left. \parallel{p\lbrack k\rbrack - p\lbrack k - 1\rbrack} \right.\parallel\mathbb{1}_{\left. \left\lbrack t\lbrack j\rbrack - \Delta T,t\lbrack j\rbrack \right\rbrack \right.}\left( t\lbrack k\rbrack \right)}{\sum_{k = 1}^{n}\left( t\lbrack k\rbrack - t\lbrack k - 1\rbrack \right)\mathbb{1}_{\left. \left\lbrack t\lbrack j\rbrack - \Delta T,t\lbrack j\rbrack \right\rbrack \right.}\left( t\lbrack k\rbrack \right)}\quad & \text{otherwise} \end{cases} Interpolate between the average position and the raw ones based on the average speed ∀ j ∈ ⟦ 0 , n ⟧ , p ′ [ j ] = min ( v ¯ [ j ] − v min v max − v min 𝟙 [ v min , ∞ [ ( v ¯ [ j ] ) , 1 ) p ¯ [ j ] + ( 1 − min ( v ¯ [ j ] − v min v max − v min 𝟙 [ v min , ∞ [ ( v ¯ [ j ] ) ) ) p [ j ] \begin{aligned} \forall j \in ⟦0,n⟧,p\prime\lbrack j\rbrack = & \min(\frac{\underset{¯}{v}\lbrack j\rbrack - v_{\text{min}}}{v_{\text{max }} - v_{\text{min}}}\mathbb{1}_{\lbrack v_{\text{min}},\infty\lbrack}\left( \underset{¯}{v}\lbrack j\rbrack \right),1)\underset{¯}{p}\lbrack j\rbrack \\ + & \left( 1 - \min(\frac{\underset{¯}{v}\lbrack j\rbrack - v_{\text{min}}}{v_{\text{max }} - v_{\text{min}}}\mathbb{1}_{\lbrack v_{\text{min}},\infty\lbrack}\left( \underset{¯}{v}\lbrack j\rbrack \right)) \right)p\lbrack j\rbrack \end{aligned} where p ′ [ j ] = ( x ′ [ j ] , y ′ [ j ] ) p\prime\lbrack j\rbrack = \left( x\prime\lbrack j\rbrack,y\prime\lbrack j\rbrack \right)
Output: { ( x ′ [ k ] , y ′ [ k ] ) ∈ ℝ 2 , 0 ≤ k ≤ n } \left\{ \left( x\prime\lbrack k\rbrack,y\prime\lbrack k\rbrack \right) \in {\mathbb{R}}^{2},0 \leq k \leq n \right\} the filtered positions.
Hence for low local speeds, the smoothing is maximum (we take exactly the average position over the time Δ T \Delta T ) and for high speed there is no smoothing. We also note that the first position is thus never filtered.