# Filter and Smoothing

Filters are indispensable in the field of robotics, enabling robots to interpret their environment and respond appropriately.

One of the most prevalent filters in signal processing is the **Low-Pass Filter (LPF)**. These filters are designed to
permit the passage of signals with frequencies below a specified threshold and to diminish the impact of higher
frequencies, which are typically regarded as undesirable noise.

Low-pass filters hold significant value in the realm of robotics for several reasons:

- Enhancing Sensor Readings: Given that a multitude of sensors are susceptible to signal disruption, LPFs are adept at refining the data, resulting in enhanced precision.
- Stabilizing Signal Processing: By mitigating abrupt fluctuations in control signals, LPFs contribute to the seamless operation of robotic components like motors and actuators.
- Noise Mitigation: LPFs are effective in curbing high-frequency disruptions originating from multiple sources, including electrical disturbances.

## Python Code for Discrete Low-Pass Filtering

Here is a straightforward Python code snippet that demonstrates the implementation of a discrete low-pass filter, commonly utilized in robotics to enhance the quality of sensor readings.

`import numpy as np`

class LowPassFilter:

def __init__(self, control_freq, cutoff_freq):

dt = 1 / control_freq

wc = 2 * np.pi * cutoff_freq

temp = 1 - np.cos(wc * dt)

self.alpha = -temp + np.sqrt(temp ** 2 + 2 * temp)

self.y = 0

self.initialized = False

def apply(self, x):

if not self.initialized:

self.init(x)

self.y += self.alpha * (x - self.y)

return self.y

def init(self, initial_value):

self.y = initial_value

self.initialized = True

This code presents a simple yet effective approach to applying a first-order discrete low-pass filter, which operates similarly to an exponential smoothing mechanism due to its straightforward computational structure.

In addition to the custom implementation, the `scipy`

library provides facilities to achieve similar results. The
following function demonstrates how to apply a low-pass filter to a series of signals, smoothing the input data
effectively.

`import numpy as np`

from scipy import signal

def smooth_signal_sequence(signal_sequence: np.ndarray, cutoff_freq=5, sample_rate=25):

sos = signal.butter(2, cutoff_freq, 'low', fs=sample_rate, output='sos')

dimensions = signal_sequence.shape

if len(dimensions) < 2:

raise ValueError(f"Expected a 2D or 3D array, received shape: {dimensions}")

smoothed_sequence = np.empty_like(signal_sequence)

if len(dimensions) == 3:

for joint_index in range(dimensions[1]):

for axis_index in range(dimensions[2]):

smoothed_sequence[:, joint_index, axis_index] = signal.sosfilt(sos, signal_sequence[:, joint_index, axis_index])

elif len(dimensions) == 2:

for joint_index in range(dimensions[1]):

smoothed_sequence[:, joint_index] = signal.sosfilt(sos, signal_sequence[:, joint_index])

return smoothed_sequence