xxxxxxxxxx
/*
First-Order Low-Pass Filter
@param [float] prev_filtered Previous Filtered Measurement
@param [float] curr_mes Current Non-Filtered Measurement
@param [float] timestep_s Timestep Between Measurements In Seconds
@param [float] cuttof_freq Desired Cuttof Frequency
*/
float filter_lowPass(float prev_filtered, float curr_mes, float timestep_s, float cuttof_freq)
{
float alpha = 1 / (1 + 2 * PI * cuttof_freq * timestep_s);
return alpha * prev_filtered + (1 - alpha) * curr_mes;
}
xxxxxxxxxx
/*
First-Order High-Pass Filter
@param [float] prev_filtered Previous Filtered Measurement
@param [float] curr_mes Current Non-Filtered Measurement
@param [float] prev_mes Previous Non-filtered Measurement
@param [float] timestep_s Timestep Between Measurements In Seconds
@param [float] cuttof_freq Desired Cuttof Frequency
*/
float filter_highPass(float prev_filtered, float curr_mes, float prev_mes, float timestep_s, float cuttof_freq)
{
float alpha = 1 - 1 / (1 + 2 * PI * cuttof_freq * timestep_s);
return (1 - alpha) * prev_filtered + (1 - alpha) * (curr_mes - prev_mes);
}