Potential Issue with Sequential Updates in savitskyGolayFilter Function

[XinyuKhan]

Description:

In the current implementation of the savitskyGolayFilter function, the Savitzky-Golay filter is applied in a sequential manner, where each updated value depends on previously updated values. This contradicts the standard approach of applying the Savitzky-Golay filter, where each value should be computed based on the original unmodified data.

For example, in the function applyFilterOverAxis, when updating sequence[idx], it uses sequence[idx-1], which may have already been updated in a previous step. This can lead to incorrect results, as the filter should always operate on the original data rather than the updated sequence.

Expected Behavior:

The filter should calculate new values based on the original sequence without using previously updated values. A possible solution is to store the filtered results in a temporary array and apply them to the sequence only after all points have been filtered.

Affected Code:

sequence(idx) = applyFilter({
  sequence(idx - 4),
  sequence(idx - 3),
  sequence(idx - 2),
  sequence(idx - 1),
  sequence(idx),
  sequence(idx + 1),
  sequence(idx + 2),
  sequence(idx + 3),
  sequence(idx + 4)
});

Suggested Fix:

Consider storing the filtered values in a separate temporary array to ensure each value is calculated using the original sequence data, and only update the sequence once all values are computed.

[SteveMacenski]

I agree. Let me noodle on this a bit for how to improve it. There's a few things about this function I don't love so I might just rewrite it all now and fix this as part of it. This can be vastly simplified

[SteveMacenski]

@XinyuKhan can you test and review #4669?

[XinyuKhan]

I also found that control sequence shift operation will set the last control history and the first element in the un-shifted sequence to the same value, that cause there are two repeated element in the filter pipeline.

[SteveMacenski]

Please comment inline the software in PR (if this is an issue wrt my new changes -- or the old method?), I'm not entire sure what you're pointing out 😄 We should fix that (if still there) before merging.

[SteveMacenski]

@XinyuKhan can you please review?

[SteveMacenski]

last control history and the first element in the un-shifted sequence to the same value

Since you didn't have time to review yet, I went through and really carefully parsed this to see what you meant. I think I understand what you mean, but I think this is correct.

  control_history[3] = {
    control_sequence.vx(offset),
    control_sequence.vy(offset),
    control_sequence.wz(offset)};
}

This sets the last control history to the control_sequence that is about to be executed on the robot hardware (i.e. either shifted or unshifted, depending on the operational mode). In the next cycle, we resample, rescore, and resmooth so the value of control_sequence at each position can mean different things -- especially in the context with the control shift to index 1 since that'll be resampled with some noise characteristics. If the value is 0 for the shift, then we use the first sample which is then rolled over after we execute the trajectory so that is also not the same.

If I understand what you mean properly, I think this points out that this is correct due to the trajectory rolling and sampling.

[SteveMacenski]

Closing this and testing + merging the PR, thanks for the report @XinyuKhan ! It also gave me a good reason to rewrite this more cleanly.