MBot Software Library
v1.0
An API documentation to mbot_firmware repository
|
Functions for generating and implementing discrete SISO filters. More...
Classes | |
struct | rc_filter_t |
Struct containing configuration and state of a SISO filter. More... | |
Macros | |
#define | RC_FILTER_INITIALIZER |
Typedefs | |
typedef struct rc_filter_t | rc_filter_t |
Struct containing configuration and state of a SISO filter. More... | |
Functions | |
rc_filter_t | rc_filter_empty (void) |
Critical function for initializing rc_filter_t structs. More... | |
int | rc_filter_alloc (rc_filter_t *f, rc_vector_t num, rc_vector_t den, double dt) |
Allocate memory for a discrete-time filter & populates it with the transfer function coefficients provided in vectors num and den. More... | |
int | rc_filter_alloc_from_arrays (rc_filter_t *f, double dt, double *num, int numlen, double *den, int denlen) |
Like rc_filter_alloc(), but takes arrays for the numerator and denominator coefficients instead of vectors. More... | |
int | rc_filter_duplicate (rc_filter_t *f, rc_filter_t old) |
duplicates a filter More... | |
int | rc_filter_free (rc_filter_t *f) |
Frees the memory allocated by a filter's buffers and coefficient vectors. Also resets all filter properties back to 0. More... | |
int | rc_filter_print (rc_filter_t f) |
Prints the transfer function and other statistic of a filter to the screen. More... | |
double | rc_filter_march (rc_filter_t *f, double new_input) |
March a filter forward one step with new input provided as an argument. More... | |
int | rc_filter_reset (rc_filter_t *f) |
Resets all previous inputs and outputs to 0. Also resets the step counter & saturation flag. More... | |
int | rc_filter_enable_saturation (rc_filter_t *f, double min, double max) |
Enables saturation between bounds min and max. More... | |
int | rc_filter_get_saturation_flag (rc_filter_t *f) |
Checks if the filter saturated the last time step. More... | |
int | rc_filter_enable_soft_start (rc_filter_t *f, double seconds) |
Enables soft start functionality where the output bound is gradually opened linearly from 0 to the normal saturation range. More... | |
double | rc_filter_previous_input (rc_filter_t *f, int steps) |
Returns the input 'steps' back in time. Steps=0 returns most recent input. More... | |
double | rc_filter_previous_output (rc_filter_t *f, int steps) |
Returns the output 'steps' back in time. Steps = 0 returns most recent output. More... | |
int | rc_filter_prefill_inputs (rc_filter_t *f, double in) |
Fills all previous inputs to the filter as if they had been equal to 'in'. More... | |
int | rc_filter_prefill_outputs (rc_filter_t *f, double out) |
Fills all previous outputs of the filter as if they had been equal to 'out'. More... | |
int | rc_filter_multiply (rc_filter_t f1, rc_filter_t f2, rc_filter_t *out) |
Creates a new filter 'out' by multiplying f1*f2. More... | |
int | rc_filter_multiply_three (rc_filter_t f1, rc_filter_t f2, rc_filter_t f3, rc_filter_t *out) |
Creates a new filter 'out' by multiplying f1*f2*f3. More... | |
int | rc_filter_c2d_tustin (rc_filter_t *f, double dt, rc_vector_t num, rc_vector_t den, double w) |
Creates a discrete time filter with similar dynamics to a provided continuous time transfer function using tustin's approximation with prewarping about a frequency of interest 'w' in radians per second. More... | |
int | rc_filter_normalize (rc_filter_t *f) |
Normalizes a discrete time filter so that the leading demoninator coefficient is 1. More... | |
int | rc_filter_first_order_lowpass (rc_filter_t *f, double dt, double tc) |
Creates a first order low pass filter. More... | |
int | rc_filter_first_order_highpass (rc_filter_t *f, double dt, double tc) |
Creates a first order high pass filter. More... | |
int | rc_filter_butterworth_lowpass (rc_filter_t *f, int order, double dt, double wc) |
Creates a Butterworth low pass filter of specified order and cutoff frequency. More... | |
int | rc_filter_butterworth_highpass (rc_filter_t *f, int order, double dt, double wc) |
Creates a Butterworth high pass filter of specified order and cutoff frequency. More... | |
int | rc_filter_moving_average (rc_filter_t *f, int samples, double dt) |
Makes a FIR moving average filter that averages over specified number of samples. More... | |
int | rc_filter_integrator (rc_filter_t *f, double dt) |
Creates a first order integrator. More... | |
int | rc_filter_double_integrator (rc_filter_t *f, double dt) |
Creates a second order double integrator. More... | |
int | rc_filter_pid (rc_filter_t *f, double kp, double ki, double kd, double Tf, double dt) |
Creates a discrete-time implementation of a parallel PID controller with high-frequency rolloff using the forward-Euler integration method. More... | |
int | rc_filter_third_order_complement (rc_filter_t *lp, rc_filter_t *hp, double freq, double damp, double dt) |
Creates a third order symmetric complementary pair of high/low pass filters. More... | |
Functions for generating and implementing discrete SISO filters.
This discrete filter implementation allows for easy implementation of arbitrary transfer functions in code. It keeps track of all previous inputs and outputs in ring buffers for efficiency. All computation is done using single-precision doubleing point values.
Several functions are providing for generating filter coefficients for the most common functions such as low/high pass filters, moving average filters, PID, and integrators.
See the rc_test_filter.c example for use case.
#define RC_FILTER_INITIALIZER |
typedef struct rc_filter_t rc_filter_t |
Struct containing configuration and state of a SISO filter.
Also points to dynamically allocated memory which make it necessary to use the allocation and free function in this API for proper use. The user can read and modify values directly from ths struct.
int rc_filter_alloc | ( | rc_filter_t * | f, |
rc_vector_t | num, | ||
rc_vector_t | den, | ||
double | dt | ||
) |
Allocate memory for a discrete-time filter & populates it with the transfer function coefficients provided in vectors num and den.
The memory in num and den is duplicated so those vectors can be reused or freed after allocating a filter without fear of disturbing the function of the filter. Argument dt is the timestep in seconds at which the user expects to operate the filter. The length of demonimator den must be at least as large as numerator num to avoid describing an improper transfer function. If rc_filter_t pointer f points to an existing filter then the old filter's contents are freed safely to avoid memory leaks. We suggest initializing filter f with rc_filter_empty before calling this function if it is not a global variable to ensure it does not accidentally contain invlaid contents such as null pointers. The filter's order is derived from the length of the denominator polynomial.
[out] | f | Pointer to user's rc_filter_t struct |
[in] | num | The numerator vector |
[in] | den | The denomenator vector |
[in] | dt | Timestep in seconds |
int rc_filter_alloc_from_arrays | ( | rc_filter_t * | f, |
double | dt, | ||
double * | num, | ||
int | numlen, | ||
double * | den, | ||
int | denlen | ||
) |
Like rc_filter_alloc(), but takes arrays for the numerator and denominator coefficients instead of vectors.
Arrays num and den must have lengths that form a proper or semi-proper transfer function.
[out] | f | Pointer to user's rc_filter_t struct |
[in] | dt | Timestep in seconds |
[in] | num | pointer to numerator array |
[in] | numlen | The numerator length |
[in] | den | pointer to denominator array |
[in] | denlen | The denominator length |
int rc_filter_butterworth_highpass | ( | rc_filter_t * | f, |
int | order, | ||
double | dt, | ||
double | wc | ||
) |
Creates a Butterworth high pass filter of specified order and cutoff frequency.
Any existing memory allocated for f is freed safely to avoid memory leaks and new memory is allocated for the new filter.
[out] | f | Pointer to user's rc_filter_t struct |
[in] | order | The order (>=1) |
[in] | dt | desired timestep of discrete filter in seconds |
[in] | wc | Cuttoff freqauency in rad/s |
int rc_filter_butterworth_lowpass | ( | rc_filter_t * | f, |
int | order, | ||
double | dt, | ||
double | wc | ||
) |
Creates a Butterworth low pass filter of specified order and cutoff frequency.
Any existing memory allocated for f is freed safely to avoid memory leaks and new memory is allocated for the new filter.
[out] | f | Pointer to user's rc_filter_t struct |
[in] | order | The order (>=1) |
[in] | dt | desired timestep of discrete filter in seconds |
[in] | wc | Cuttoff freqauency in rad/s |
int rc_filter_c2d_tustin | ( | rc_filter_t * | f, |
double | dt, | ||
rc_vector_t | num, | ||
rc_vector_t | den, | ||
double | w | ||
) |
Creates a discrete time filter with similar dynamics to a provided continuous time transfer function using tustin's approximation with prewarping about a frequency of interest 'w' in radians per second.
Any existing memory allocated for f is freed is necessary to prevent memory leaks. Returns 0 on success or -1 on failure.
[out] | f | Pointer to user's rc_filter_t struct |
[in] | dt | desired timestep of discrete filter in seconds |
[in] | num | continuous time numerator coefficients |
[in] | den | continuous time denominator coefficients |
[in] | w | prewarping frequency in rad/s |
int rc_filter_double_integrator | ( | rc_filter_t * | f, |
double | dt | ||
) |
Creates a second order double integrator.
Like most functions here, the dynamics are only accurate if the filter is called with a timestep corresponding to dt. Any existing memory allocated for f is freed safely to avoid memory leaks and new memory is allocated for the new filter.
[out] | f | Pointer to user's rc_filter_t struct |
[in] | dt | desired timestep of discrete filter in seconds |
int rc_filter_duplicate | ( | rc_filter_t * | f, |
rc_filter_t | old | ||
) |
duplicates a filter
This allocates new memory in filter f so it can be used independently from the old filter but with the same configuration.
f | new filter to allocate | |
[in] | old | old filter to copy |
rc_filter_t rc_filter_empty | ( | void | ) |
Critical function for initializing rc_filter_t structs.
This is a very important function. If your rc_filter_t struct is not a global variable, then its initial contents cannot be guaranteed to be anything in particular. Therefore it could contain problematic contents which could interfere with functions in this library. Therefore, you should always initialize your filters with rc_filter_empty before using with any other function in this library such as rc_filter_alloc. This serves the same purpose as rc_matrix_empty, rc_vector_empty, and rc_ringbuf_empty.
int rc_filter_enable_saturation | ( | rc_filter_t * | f, |
double | min, | ||
double | max | ||
) |
Enables saturation between bounds min and max.
If saturation is enabled for a specified filter, the filter will automatically bound the output between min and max. You may ignore this function if you wish the filter to run unbounded. Max must be greater than or equal to min. If max==min, the output will be fixed at that value. Any double-precision floating point value is allowed, positive or negative.
f | Pointer to user's rc_filter_t struct | |
[in] | min | The lower bound |
[in] | max | The upper bound |
int rc_filter_enable_soft_start | ( | rc_filter_t * | f, |
double | seconds | ||
) |
Enables soft start functionality where the output bound is gradually opened linearly from 0 to the normal saturation range.
This occurs over the time specified from argument 'seconds' from when the filter is first created or reset. Saturation must already be enabled for this to work. This assumes that the user does indeed call rc_filter_march at roughly the same time interval as the 'dt' variable in the filter struct which is set at creation time. The soft-start property is maintained through a call to rc_filter_reset so the filter will soft-start again after each reset. This feature should only really be used for feedback controllers to prevent jerky starts. The saturation flag will not be set during this period as the output is usually expected to be bounded and we don't want to falsely trigger alarms or saturation counters.
f | Pointer to user's rc_filter_t struct | |
[in] | seconds | Time in seconds |
int rc_filter_first_order_highpass | ( | rc_filter_t * | f, |
double | dt, | ||
double | tc | ||
) |
Creates a first order high pass filter.
Any existing memory allocated for f is freed safely to avoid memory leaks and new memory is allocated for the new filter. dt is in units of seconds and time_constant is the number of seconds it takes to decay by 63.4% of a steady-state input. This can be used alongside rc_first_order_highpass to make a complementary filter pair.
[out] | f | Pointer to user's rc_filter_t struct |
[in] | dt | desired timestep of discrete filter in seconds |
[in] | tc | time constant: Seconds it takes to decay by 63.4% of a steady-state input |
int rc_filter_first_order_lowpass | ( | rc_filter_t * | f, |
double | dt, | ||
double | tc | ||
) |
Creates a first order low pass filter.
Any existing memory allocated for f is freed safely to avoid memory leaks and new memory is allocated for the new filter. dt is in units of seconds and time_constant is the number of seconds it takes to rise to 63.4% of a steady-state input. This can be used alongside rc_first_order_highpass to make a complementary filter pair.
[out] | f | Pointer to user's rc_filter_t struct |
[in] | dt | desired timestep of discrete filter in seconds |
[in] | tc | time constant: Seconds it takes to rise to 63.4% of a steady-state input |
int rc_filter_free | ( | rc_filter_t * | f | ) |
Frees the memory allocated by a filter's buffers and coefficient vectors. Also resets all filter properties back to 0.
f | Pointer to user's rc_filter_t struct |
int rc_filter_get_saturation_flag | ( | rc_filter_t * | f | ) |
Checks if the filter saturated the last time step.
This information could also be retrieved by looking at the 'sat_flag' value in the filter struct.
f | Pointer to user's rc_filter_t struct |
int rc_filter_integrator | ( | rc_filter_t * | f, |
double | dt | ||
) |
Creates a first order integrator.
Like most functions here, the dynamics are only accurate if the filter is called with a timestep corresponding to dt. Any existing memory allocated for f is freed safely to avoid memory leaks and new memory is allocated for the new filter.
[out] | f | Pointer to user's rc_filter_t struct |
[in] | dt | desired timestep of discrete filter in seconds |
double rc_filter_march | ( | rc_filter_t * | f, |
double | new_input | ||
) |
March a filter forward one step with new input provided as an argument.
If saturation or soft-start are enabled then the output will automatically be bound appropriately. The steps counter is incremented by one and internal ring buffers are updated accordingly. Once a filter is created, this is typically the only function required afterwards.
f | Pointer to user's rc_filter_t struct | |
[in] | new_input | The new input |
int rc_filter_moving_average | ( | rc_filter_t * | f, |
int | samples, | ||
double | dt | ||
) |
Makes a FIR moving average filter that averages over specified number of samples.
Any existing memory allocated for f is freed safely to avoid memory leaks and new memory is allocated for the new filter.
Note that the timestep dt does not effect the dynamics of the produced filter. It is simply copied into the 'dt' field of the rc_filter_t struct. However, it is necessary for creation of this filter for compatability with the soft-start feature and any other user codepaths that may be dependent on a filter's timestep.
[out] | f | Pointer to user's rc_filter_t struct |
[in] | samples | The samples to average over (>=2) |
[in] | dt | desired timestep of discrete filter in seconds |
int rc_filter_multiply | ( | rc_filter_t | f1, |
rc_filter_t | f2, | ||
rc_filter_t * | out | ||
) |
Creates a new filter 'out' by multiplying f1*f2.
The contents of f3 are freed safely if necessary and new memory is allocated to avoid memory leaks.
[in] | f1 | Pointer to user's rc_filter_t struct to be multiplied |
[in] | f2 | Pointer to user's rc_filter_t struct to be multiplied |
[out] | out | Pointer to newly created filter struct |
int rc_filter_multiply_three | ( | rc_filter_t | f1, |
rc_filter_t | f2, | ||
rc_filter_t | f3, | ||
rc_filter_t * | out | ||
) |
Creates a new filter 'out' by multiplying f1*f2*f3.
The contents of f3 are freed safely if necessary and new memory is allocated to avoid memory leaks.
[in] | f1 | Pointer to user's rc_filter_t struct to be multiplied |
[in] | f2 | Pointer to user's rc_filter_t struct to be multiplied |
[in] | f3 | Pointer to user's rc_filter_t struct to be multiplied |
[out] | out | Pointer to newly created filter struct |
int rc_filter_normalize | ( | rc_filter_t * | f | ) |
Normalizes a discrete time filter so that the leading demoninator coefficient is 1.
f | Pointer to user's rc_filter_t struct |
int rc_filter_pid | ( | rc_filter_t * | f, |
double | kp, | ||
double | ki, | ||
double | kd, | ||
double | Tf, | ||
double | dt | ||
) |
Creates a discrete-time implementation of a parallel PID controller with high-frequency rolloff using the forward-Euler integration method.
This is equivalent to the Matlab function: C = pid(Kp,Ki,Kd,Tf,Ts)
It is not possible to implement a pure differentiator with a discrete transfer function so this filter has high frequency rolloff with time constant Tf. Smaller Tf results in less rolloff, but Tf must be greater than dt/2 for stability. Returns 0 on success or -1 on failure.
f | Pointer to user's rc_filter_t struct | |
[in] | kp | Proportional constant |
[in] | ki | Integration constant |
[in] | kd | Derivative constant |
[in] | Tf | High Frequency rolloff time constant (seconds) |
[in] | dt | desired timestep of discrete filter in seconds |
int rc_filter_prefill_inputs | ( | rc_filter_t * | f, |
double | in | ||
) |
Fills all previous inputs to the filter as if they had been equal to 'in'.
Most useful when starting high-pass filters to prevent unwanted jumps in the output when starting with non-zero input.
f | Pointer to user's rc_filter_t struct | |
[in] | in | Input value to fill |
int rc_filter_prefill_outputs | ( | rc_filter_t * | f, |
double | out | ||
) |
Fills all previous outputs of the filter as if they had been equal to 'out'.
Most useful when starting low-pass filters to prevent unwanted settling time when starting with non-zero input.
f | Pointer to user's rc_filter_t struct | |
[in] | out | output value to fill |
double rc_filter_previous_input | ( | rc_filter_t * | f, |
int | steps | ||
) |
Returns the input 'steps' back in time. Steps=0 returns most recent input.
'steps' must be between 0 and order inclusively as those are the only steps retained in memory for normal filter operation. To record values further back in time we suggest creating your own rc_ringbuf_t ring buffer.
f | Pointer to user's rc_filter_t struct | |
[in] | steps | The steps back in time, steps=0 returns most recent input. |
double rc_filter_previous_output | ( | rc_filter_t * | f, |
int | steps | ||
) |
Returns the output 'steps' back in time. Steps = 0 returns most recent output.
'steps' must be between 0 and order inclusively as those are the only steps retained in memory for normal filter operation. To record values further back in time we suggest creating your own rc_ringbuf_t ring buffer.
f | Pointer to user's rc_filter_t struct | |
[in] | steps | The steps back in time, steps=0 returns most recent output. |
int rc_filter_print | ( | rc_filter_t | f | ) |
Prints the transfer function and other statistic of a filter to the screen.
Only works on filters up to order 9.
f | Pointer to user's rc_filter_t struct |
int rc_filter_reset | ( | rc_filter_t * | f | ) |
Resets all previous inputs and outputs to 0. Also resets the step counter & saturation flag.
This is sufficient to start the filter again as if it were just created.
f | Pointer to user's rc_filter_t struct |
int rc_filter_third_order_complement | ( | rc_filter_t * | lp, |
rc_filter_t * | hp, | ||
double | freq, | ||
double | damp, | ||
double | dt | ||
) |
Creates a third order symmetric complementary pair of high/low pass filters.
lp | lowpass filter to be populated | |
hp | highpass filter to be populated | |
[in] | freq | crossover frequency in rad/s |
[in] | damp | Damping ratio >0, 1 is critically damped, lower than that gets wobbly |
[in] | dt | desired timestep of discrete filter in seconds |