acc_algorithm.h
Go to the documentation of this file.
1 // Copyright (c) Acconeer AB, 2023-2025
2 // All rights reserved
3 
4 #ifndef ACC_ALGORITHM_H_
5 #define ACC_ALGORITHM_H_
6 
7 #include <complex.h>
8 #include <stdint.h>
9 
10 #include "acc_config.h"
11 #include "acc_definitions_a121.h"
12 #include "acc_definitions_common.h"
13 
14 /**
15  * @brief Approximate minimum step length for a sensor measurement in meters
16  *
17  * Used for meter to point conversion
18  */
19 #define ACC_APPROX_BASE_STEP_LENGTH_M (2.5e-3f)
20 
21 /**
22  * @brief Roll array elements and push new element last
23  *
24  * @param[in, out] data Array to be rolled
25  * @param[in] data_length Length of the array
26  * @param[in] element The new element
27  */
28 void acc_algorithm_roll_and_push(float *data, uint16_t data_length, float element);
29 
30 /**
31  * @brief Roll row elements and push a new column
32  *
33  * @param[in, out] data Matrix to be rolled of size rows*cols
34  * @param[in] rows Number of rows in the matrix
35  * @param[in] cols Number of cols in the matrix
36  * @param[in] column The new column
37  * @param[in] pos_shift If true will be the same as shift=1 in np.roll, otherwise the same as shift=-1
38  */
39 void acc_algorithm_roll_and_push_matrix_f32(float *data, uint16_t rows, uint16_t cols, const float *column, bool pos_shift);
40 
41 /**
42  * @brief Roll row elements and push a new column
43  *
44  * @param[in, out] data Matrix to be rolled of size rows*cols
45  * @param[in] rows Number of rows in the matrix
46  * @param[in] cols Number of cols in the matrix
47  * @param[in] column The new column
48  * @param[in] pos_shift If true will be the same as shift=1 in np.roll, otherwise the same as shift=-1
49  */
50 void acc_algorithm_roll_and_push_matrix_f32_complex(float complex *data, uint16_t rows, uint16_t cols, const float complex *column, bool pos_shift);
51 
52 /**
53  * @brief Roll row elements and push multiple columns
54  *
55  * @param[in, out] data Matrix to be rolled
56  * @param[in] data_rows Number of rows in data matrix
57  * @param[in] cols Number of cols in data matrix
58  * @param[in] matrix Matrix with new columns
59  * @param[in] matrix_rows Number of rows in the matrix
60  * @param[in] pos_shift If true will be the same as shift=1 in np.roll, otherwise the same as shift=-1
61  */
62 void acc_algorithm_roll_and_push_mult_matrix_i16_complex(acc_int16_complex_t *data,
63  uint16_t data_rows,
64  uint16_t cols,
65  const acc_int16_complex_t *matrix,
66  uint16_t matrix_rows,
67  bool pos_shift);
68 
69 /**
70  * @brief Unwraps a signal by changing elements which have an absolute difference from
71  * their predecessor of more than 2*pi to their period-complementary values.
72  *
73  * @param[in, out] data Array with signal data
74  * @param[in] data_length Length of the array
75  */
76 void acc_algorithm_unwrap(float *data, uint16_t data_length);
77 
78 /**
79  * @brief Find index of largest element in the array
80  *
81  * @param[in] data Array of data
82  * @param[in] data_length Length of the array
83  * @return Index of largest element
84  */
85 uint16_t acc_algorithm_argmax(const float *data, uint16_t data_length);
86 
87 /**
88  * @brief Interpolate peak
89  *
90  * @param[in] y Amplitudes of frequency peaks
91  * @param[in] x Frequencies
92  * @return Interpolated frequency
93  */
94 float acc_algorithm_interpolate_peaks(const float *y, const float *x);
95 
96 /**
97  * @brief Interpolate equidistant peaks
98  *
99  * The function fits a second degree polynomial to three consecutive amplitude
100  * values where the second element is expected to contain the maximum measured amplitude.
101  * The function then finds the position of the maximum amplitude of the polynomial.
102  * The position is normalized.
103  *
104  * @param[in] y Y values to interpolate
105  * @param[in] x_start Start value of X-axis
106  * @param[in] x_delta Delta between values on X-axis
107  * @param[in] peak_idx Idx of peak
108  * @return Interpolated X-values
109  */
110 float acc_algorithm_interpolate_peaks_equidistant(const float *y, float x_start, float x_delta, uint16_t peak_idx);
111 
112 /**
113  * @brief Design a 2nd order digital Butterworth lowpass filter
114  *
115  * @param[in] freq Cutoff freuency
116  * @param[in] fs Sampling frequency, > 0 Hz
117  * @param[out] b Numerator in polynomial of the IIR filter, length == 3
118  * @param[out] a Denominator of polynomial of the IIR filter, length == 2
119  */
120 void acc_algorithm_butter_lowpass(float freq, float fs, float *b, float *a);
121 
122 /**
123  * @brief Design a 2nd order digital Butterworth bandpass filter
124  *
125  * @param[in] min_freq Low cutoff frequency
126  * @param[in] max_freq High cutoff frequency
127  * @param[in] fs Sampling frequency, > 0 Hz
128  * @param[out] b Numerator in polynomial of the IIR filter, length == 5
129  * @param[out] a Denominator of polynomial of the IIR filter, length == 4
130  */
131 void acc_algorithm_butter_bandpass(float min_freq, float max_freq, float fs, float *b, float *a);
132 
133 /**
134  * @brief Filter data with a digital filter
135  *
136  * @param[in] b Numerator in polynomial of the IIR filter, length == 5
137  * @param[in] a Denominator of polynomial of the IIR filter, length == 4
138  * @param[in, out] data Data array to filter
139  * @param[in] data_length Length of the array
140  */
141 void acc_algorithm_lfilter(const float *b, const float *a, float *data, uint16_t data_length);
142 
143 /**
144  * @brief Filter data along row dimension
145  *
146  * @param[in] b Numerator in polynomial of the IIR filter, length == 5
147  * @param[in] a Denominator of polynomial of the IIR filter, length == 4
148  * @param[in, out] data Matrix to filter
149  * @param[in] rows Number of rows in the matrix
150  * @param[in] cols Number of columns in the matrix
151  */
152 void acc_algorithm_lfilter_matrix(const float *b, const float *a, float *data, uint16_t rows, uint16_t cols);
153 
154 /**
155  * @brief Apply filter coefficients to filtered data matrix and data matrix
156  *
157  * @param[in] a Denominator of polynomial of the IIR filter
158  * @param[in] filt_data Filtered data matrix
159  * @param[in] filt_rows Number of rows in filtered data matrix, == len(a)
160  * @param[in] filt_cols Number of columns in filtered data matrix, == data_cols
161  * @param[in] b Numerator of polynomial of the IIR filter
162  * @param[in] data Data matrix
163  * @param[in] data_rows Number of rows in data matrix, == len(b)
164  * @param[in] data_cols Number of columns in data matrix, == filt_cols
165  * @param[out] output Output filtered data array
166  * @param[in] output_length Length of output, == data_cols and filt_cols
167  */
168 void acc_algorithm_apply_filter_f32(const float *a,
169  const float *filt_data,
170  uint16_t filt_rows,
171  uint16_t filt_cols,
172  const float *b,
173  const float *data,
174  uint16_t data_rows,
175  uint16_t data_cols,
176  float *output,
177  uint16_t output_length);
178 
179 /**
180  * @brief Apply filter coefficients to filtered data matrix and data matrix
181  *
182  * @param[in] a Denominator of polynomial of the IIR filter
183  * @param[in] filt_data Filtered data matrix
184  * @param[in] filt_rows Number of rows in filtered data matrix, == len(a)
185  * @param[in] filt_cols Number of columns in filtered data matrix, == data_cols
186  * @param[in] b Numerator of polynomial of the IIR filter
187  * @param[in] data Data matrix
188  * @param[in] data_rows Number of rows in data matrix, == len(b)
189  * @param[in] data_cols Number of columns in data matrix, == filt_cols
190  * @param[out] output Output filtered data array
191  * @param[in] output_length Length of output, == data_cols and filt_cols
192  */
193 void acc_algorithm_apply_filter_f32_complex(const float *a,
194  const float complex *filt_data,
195  uint16_t filt_rows,
196  uint16_t filt_cols,
197  const float *b,
198  const float complex *data,
199  uint16_t data_rows,
200  uint16_t data_cols,
201  float complex *output,
202  uint16_t output_length);
203 
204 /**
205  * @brief Calculate mean sweep of a frame from start_point to end_point
206  *
207  * @param[in] frame Frame to calculate mean sweep for
208  * @param[in] num_points Number of points in a sweep
209  * @param[in] sweeps_per_frame Number of sweeps in the frame
210  * @param[in] start_point Start point of mean sweep, if 0 will be same start point as for sweeps in frame
211  * @param[in] end_point End point of mean sweep, if num_points will be same end point as for sweeps in frame
212  * @param[out] sweep Mean sweep returned from calculation, length >= (end_point - start_point)
213  */
214 void acc_algorithm_mean_sweep(const acc_int16_complex_t *frame,
215  uint16_t num_points,
216  uint16_t sweeps_per_frame,
217  uint16_t start_point,
218  uint16_t end_point,
219  float complex *sweep);
220 
221 /**
222  * @brief Calculate the sum of all sweeps in a frame, from start_point to end_point
223  *
224  * @param[in] frame Frame to calculate summed sweep for
225  * @param[in] num_points Number of points in a sweep
226  * @param[in] sweeps_per_frame Number of sweeps in the frame
227  * @param[in] start_point Start point of sum sweep, if 0 will be same start point as for sweeps in frame
228  * @param[in] end_point End point of sum sweep, if num_points will be same end point as for sweeps in frame
229  * @param[out] sum_sweep Summed sweep returned from calculation, length >= (end_point - start_point)
230  */
231 void acc_algorithm_sum_sweep(const acc_int16_complex_t *frame,
232  uint16_t num_points,
233  uint16_t sweeps_per_frame,
234  uint16_t start_point,
235  uint16_t end_point,
236  float complex *sum_sweep);
237 
238 /**
239  * @brief Calculate mean value of an array
240  *
241  * @param[in] data Data array
242  * @param[in] data_length Length of data array
243  * @param[out] out Output mean value
244  */
245 void acc_algorithm_mean_i16_complex(const acc_int16_complex_t *data, uint16_t data_length, float complex *out);
246 
247 /**
248  * @brief Calculate mean array of a matrix
249  *
250  * @param[in] matrix Matrix of data
251  * @param[in] rows Number of rows in matrix
252  * @param[in] cols Number of columns in matrix
253  * @param[out] out Output mean array, length = cols if axis = 0 or length = rows if axis = 1
254  * @param[in] axis Axis over which to calculate mean, must be 0 or 1
255  */
256 void acc_algorithm_mean_matrix_i16_complex(const acc_int16_complex_t *matrix, uint16_t rows, uint16_t cols, float complex *out, uint16_t axis);
257 
258 /**
259  * @brief Inline calculate conjugate of all elements in an array
260  *
261  * @param[in, out] data Data array, will be replaced with conjugate values
262  * @param[in] data_length Length of data array
263  */
264 void acc_algorithm_conj_f32(float complex *data, uint16_t data_length);
265 
266 /**
267  * @brief Normalize all elements in an array individually
268  *
269  * @param[in, out] data Data array, will be replaced by normalized values
270  * @param[in] data_length Length of data array
271  */
272 void acc_algorithm_normalize_f32_complex(float complex *data, uint16_t data_length);
273 
274 /**
275  * @brief 1D Fast Fourier Transform for real input
276  *
277  * @param[in] data Array of data
278  * @param[in] data_length Length of data
279  * @param[in] length_shift Integer that specifies the transform length N in accordance with N = (1 << length_shift) and N >= data_length
280  * @param[out] output Array for output data, length >= (data_length / 2) + 1
281  */
282 void acc_algorithm_rfft(const float *data, uint16_t data_length, uint16_t length_shift, float complex *output);
283 
284 /**
285  * @brief 1D Fast Fourier Transform for real input matrix
286  *
287  * @param[in] data Matrix of data
288  * @param[in] rows Number of rows in the matrix
289  * @param[in] cols Number of columns in the matrix
290  * @param[in] length_shift Integer that specifies the transform length N in accordance with N = (1 << length_shift) and N >= rows if axis == 0 or N >= cols if axis == 1
291  * @param[out] output Matrix for output data, size = ((rows / 2) + 1, cols) if axis=0 and size = (rows, (cols / 2) + 1) if axis=1
292  * @param[in] axis Axis over which to calculate the FFT, must be 0 or 1
293  */
294 void acc_algorithm_rfft_matrix(const float *data, uint16_t rows, uint16_t cols, uint16_t length_shift, float complex *output, uint16_t axis);
295 
296 /**
297  * @brief 1D Fast Fourier Transform for complex input
298  *
299  * @param[in] data Matrix of data
300  * @param[in] data_length Length of data
301  * @param[in] length_shift Integer that specifies the transform length N in accordance with N = (1 << length_shift) and N >= rows if axis == 0 or N >= cols if axis == 1
302  * @param[out] output Array for output data, must be of length N
303  */
304 void acc_algorithm_fft(const float complex *data, uint16_t data_length, uint16_t length_shift, float complex *output);
305 
306 /**
307  * @brief 1D Fast Fourier Transform for input matrix
308  *
309  * @param[in] data Matrix of data
310  * @param[in] rows Number of rows in the matrix
311  * @param[in] cols Number of columns in the matrix
312  * @param[in] length_shift Integer that specifies the transform length N in accordance with N = (1 << length_shift) and N >= rows if axis == 0 or N >= cols if axis == 1
313  * @param[out] output Matrix for output data, size = (N, cols) if axis=0 and size = (rows, N) if axis=1
314  * @param[in] axis Axis over which to calculate the FFT, must be 0 or 1
315  */
316 void acc_algorithm_fft_matrix(const float complex *data, uint16_t rows, uint16_t cols, uint16_t length_shift, float complex *output, uint16_t axis);
317 
318 /** @brief Calculate delta between frequency bins in rfft
319  *
320  * @param[in] n Window length, > 0
321  * @param[in] d Sample spacing, > 0
322  * @return Delta between frequency bins
323  */
324 float acc_algorithm_fftfreq_delta(uint16_t n, float d);
325 
326 /**
327  * @brief Calculate the real Fast Fourier Transform sample frequencies
328  *
329  * @param[in] n Window length, > 0
330  * @param[in] d Sample spacing, > 0
331  * @param[out] freqs Sample frequencies, length >= (n / 2) + 1
332  */
333 void acc_algorithm_rfftfreq(uint16_t n, float d, float *freqs);
334 
335 /**
336  * @brief Calculate the Fast Fourier Transform sample frequencies
337  *
338  * @param[in] n Window length, > 0
339  * @param[in] d Sample spacing, > 0
340  * @param[out] freqs Sample frequencies, length >= n
341  */
342 void acc_algorithm_fftfreq(uint16_t n, float d, float *freqs);
343 
344 /**
345  * @brief Calculate exponential smoothing coefficient
346  *
347  * @param[in] fs Sampling frequency
348  * @param[in] tc Time constant
349  *
350  * @return Exponential smoothing coefficient, NAN if fs=0 or tc=0
351  */
352 float acc_algorithm_exp_smoothing_coefficient(float fs, float tc);
353 
354 /**
355  * @brief Divide complex number num / denum
356  *
357  * @param[in] num Numerator
358  * @param[in] denom Denominator
359  *
360  * @return num / denom
361  */
362 float complex acc_algorithm_cdiv(float complex num, float complex denom);
363 
364 /**
365  * @brief Calculate hamming window for a specified number of points
366  *
367  * @param[in] n Number of points
368  * @param[out] window Returned hamming window, length = n
369  */
370 void acc_algorithm_hamming(uint16_t n, float *window);
371 
372 /**
373  * @brief Calculate non-symmetrical hann window for a specified number of points
374  *
375  * @param[in] n Number of points
376  * @param[out] window Returned hann window, length = n
377  */
378 void acc_algorithm_hann(uint16_t n, float *window);
379 
380 /**
381  * @brief Get the envelope Full Width Half Maximum in meters given a profile
382  *
383  * @param[in] profile The profile to get envelope FWHM for
384  * @return The envelope FWHM in meters or 0.0 if profile is not valid.
385  */
386 float acc_algorithm_get_fwhm(acc_config_profile_t profile);
387 
388 /**
389  * @brief Double buffering frame filter
390  *
391  * Detects and removes outliers in data that appear when the double buffering mode is enabled,
392  * and returns the filtered frame.
393  *
394  * Outliers are detected along the sweep dimension using the second order difference. For
395  * reliable outlier detection, the filter is applied only when there are 32 or more sweeps per frame.
396  *
397  * The disturbance caused by enabling the double buffering mode can appear in multiple sweeps
398  * but, according to observations, is limited to a maximum of two consecutive sweeps. Therefore, the
399  * function removes outliers by interpolating between the sample before and the sample two positions
400  * ahead.
401  *
402  * The function does not correct disturbances that may appear in the initial or final sweeps.
403  *
404  * @param[in, out] frame Data frame to where the filter is applied
405  * @param[in] sweeps_per_frame How many sweeps there are in the frame
406  * @param[in] num_points The number of points in the frame
407  * @param[in] work_buffer A work buffer for the filter, length >= (sweeps_per_frame - 2)
408  */
409 void acc_algorithm_double_buffering_frame_filter(acc_int16_complex_t *frame,
410  const uint16_t sweeps_per_frame,
411  const uint16_t num_points,
412  int32_t *work_buffer);
413 
414 /**
415  * @brief Shift the zero-frequency component to the center along row dimensions
416  *
417  * @param[in, out] data Matrix to be shifted
418  * @param[in] rows Number of rows in the matrix
419  * @param[in] cols Number of cols in the matrix
420  */
421 void acc_algorithm_fftshift_matrix(float *data, uint16_t rows, uint16_t cols);
422 
423 /**
424  * @brief Shift the zero-frequency component to the center
425  *
426  * @param[in, out] data Array of data
427  * @param[in] data_length Length of data
428  */
429 void acc_algorithm_fftshift(float *data, uint16_t data_length);
430 
431 /**
432  * @brief Estimate power spectral density (PSD) using Welch’s method along row dimensions
433  *
434  * See @ref acc_algorithm_welch for more details
435  *
436  * @param[in] data Matrix of data
437  * @param[in] rows Number of rows in the matrix
438  * @param[in] cols Number of cols in the matrix
439  * @param[in] segment_length Length of each segment
440  * @param[in] data_buffer Buffer used for calculations, length = segment_length
441  * @param[out] fft_out Array for fft output data, must be of length N
442  * @param[out] psds Matrix for output data, size = (cols, segment_length)
443  * @param[in] window Desired window to use, length = segment_length
444  * @param[in] length_shift Integer that specifies the transform length N in accordance with N = 1 << length_shift and N >= segment_length
445  * @param[in] fs Sampling frequency
446  */
447 void acc_algorithm_welch_matrix(const float complex *data,
448  uint16_t rows,
449  uint16_t cols,
450  uint16_t segment_length,
451  float complex *data_buffer,
452  float complex *fft_out,
453  float *psds,
454  const float *window,
455  uint16_t length_shift,
456  float fs);
457 
458 /**
459  * @brief Estimate power spectral density using Welch’s method
460  *
461  * Computes an estimate of the PSD by dividing the data into non-overlapping segments,
462  * computing a periodogram for each segment and averaging the periodograms.
463  *
464  * @param[in] data Array of data
465  * @param[in] data_length Length of data
466  * @param[in] segment_length Length of each segment
467  * @param[in] data_buffer Buffer used for calculations, length = segment_length
468  * @param[out] fft_out Array for fft output data, must be of length N
469  * @param[out] psd Array for output data, length = segment_length
470  * @param[in] window Desired window to use, length = segment_length
471  * @param[in] length_shift Integer that specifies the transform length N in accordance with N = 1 << length_shift and N >= segment_length
472  * @param[in] fs Sampling frequency
473  */
474 void acc_algorithm_welch(const float complex *data,
475  uint16_t data_length,
476  uint16_t segment_length,
477  float complex *data_buffer,
478  float complex *fft_out,
479  float *psd,
480  const float *window,
481  uint16_t length_shift,
482  float fs);
483 
484 /**
485  * @brief Calculate CFAR threshold
486  *
487  * @param[in] data Array of data
488  * @param[in] data_length Length of the data array
489  * @param[in] window_length Number of frequency bins next to the CFAR guard from which the threshold level will be calculated
490  * @param[in] half_guard_length Number of frequency bins around the point of interest that is omitted when calculating the CFAR threshold
491  * @param[in] sensitivity Sensitivity of the CFAR threshold
492  * @param[in] idx Index to calculate cfar for
493  * @return Threshold value at index
494  */
495 float acc_algorithm_calculate_cfar(const float *data,
496  uint16_t data_length,
497  uint16_t window_length,
498  uint16_t half_guard_length,
499  float sensitivity,
500  uint16_t idx);
501 
502 /**
503  * @brief Calculate mirrored one sided CFAR threshold
504  *
505  * Calculate a one sided CFAR. Guard and window are mirrored on the right half of
506  * the data. The threshold is extended to also include the CFAR margin. The
507  * margin is of size half_guard_length + window_length.
508  *
509  * On the left half this construction is used
510  * | Window | Half guard | t(i) |
511  *
512  * and on the right half this contruction is used
513  * | t(i) | Half guard | Window |
514  *
515  * The threshold extension looks like this:
516  * threshold = [t(m), t(m) ... t(m) t(m+1) t(m+2) ... t(n-m-2) t(n-m-1) t(n-m) ... t(n-m) t(n-m)]
517  * where n = data_length and m = window_length + half_guard_length
518  *
519  * @param[in] data Array of data
520  * @param[in] data_length Length of the data array
521  * @param[in] middle_idx Middle index
522  * @param[in] window_length Number of frequency bins next to the CFAR guard from which the threshold level will be calculated
523  * @param[in] half_guard_length Number of frequency bins around the point of interest that is omitted when calculating the CFAR threshold
524  * @param[in] sensitivity Sensitivity of the CFAR threshold
525  * @param[in] idx Index to calculate cfar for
526  * @return Threshold value at index
527  */
528 float acc_algorithm_calculate_mirrored_one_sided_cfar(const float *data,
529  uint16_t data_length,
530  uint16_t middle_idx,
531  uint16_t window_length,
532  uint16_t half_guard_length,
533  float sensitivity,
534  uint16_t idx);
535 
536 /**
537  * @brief Find the index of the distance column containing the largest amplitude, disregarding amplitudes present in the slow zone
538  *
539  * @param[in] data Matrix of data
540  * @param[in] cols Number of cols
541  * @param[in] rows Number of rows
542  * @param[in] middle_idx Middle index
543  * @param[in] half_slow_zone Half size of the number of frequency bins that are regarded as the slow zone
544  * @return Distance index
545  */
546 uint16_t acc_algorithm_get_distance_idx(const float *data, uint16_t cols, uint16_t rows, uint16_t middle_idx, uint16_t half_slow_zone);
547 
548 /**
549  * @brief Find the velocity of the peak with the largest amplitude, prioritizing peaks with a velocity over the slow zone limit
550  *
551  * @param[in] velocities Array of velocities
552  * @param[in] energies Array of energies
553  * @param[in] peak_idxs Indices of identified peaks
554  * @param[in] num_peaks Number of identified peaks
555  * @param[in] limit Slow zone limit
556  * @return Peak velocity
557  */
558 float acc_algorithm_get_peak_velocity(const float *velocities, const float *energies, const uint16_t *peak_idxs, uint16_t num_peaks, float limit);
559 
560 /**
561  * @brief Merges peaks
562  *
563  * @param[in] max_peak_separation The greatest distance (in meters) between peaks that will result in a merge
564  * @param[in] velocities The velocities to merge
565  * @param[in] energies The energies to merge
566  * @param[in] peak_idxs Indices of identified peaks
567  * @param[in] num_peaks Number of peaks in the peak_idxs array, if 0 nothing will happen
568  * @param[out] merged_velocities Output array for the merged velocities
569  * @param[out] merged_energies Output array for the merged energies
570  * @param[in] merged_peaks_length The length of the merged_velocities and merged_energies arrays
571  * @param[out] num_merged_peaks The number of peaks that were merged
572  * @return true if successful, false otherwise
573  */
574 bool acc_algorithm_merge_peaks(float max_peak_separation,
575  const float *velocities,
576  const float *energies,
577  const uint16_t *peak_idxs,
578  uint16_t num_peaks,
579  float *merged_velocities,
580  float *merged_energies,
581  uint16_t merged_peaks_length,
582  uint16_t *num_merged_peaks);
583 
584 /**
585  * @brief Calculate distance for a point at an index
586  *
587  * @param[in] step_length Step length in points
588  * @param[in] start_point Start point
589  * @param[in] base_step_length_m Base step length
590  * @param[in] idx Distance index
591  * @return Distance at index
592  */
593 float acc_algorithm_get_distance_m(uint16_t step_length, uint16_t start_point, float base_step_length_m, uint16_t idx);
594 
595 /**
596  * @brief Select the highest possible profile without interference of direct leakage
597  *
598  * @param[in] start_point Start point
599  * @param[in] base_step_length base_step_length
600  * @return A suitable profile
601  */
602 acc_config_profile_t acc_algorithm_select_profile(int32_t start_point, float base_step_length);
603 
604 /**
605  * @brief Select a suitable PRF given a breakpoint and profile
606  *
607  * @param[in] breakpoint A base step, relative to start_point = 0
608  * @param[in] profile The profile at breakpoint
609  * @param[in] base_step_length The base step length
610  * @return A suitable PRF
611  */
612 acc_config_prf_t acc_algorithm_select_prf(int16_t breakpoint, acc_config_profile_t profile, float base_step_length);
613 
614 /**
615  * @brief Find peaks above threshold
616  *
617  * A peak is defined as a point with greater value than its two neighbouring
618  * points and all three points are above the threshold.
619  *
620  * @param[in] abs_sweep Absolute values of the mean sweep
621  * @param[in] data_length Number of values in the sweep
622  * @param[in] threshold_check Bit array with information if peak is above or below threshold
623  * @param[out] peak_idxs Indexes of found peaks
624  * @param[in] peak_idxs_length Length of the found peaks array. To fit all possible
625  * peaks the length must be (abs_sweep_length / 2)
626  * @param[out] num_peaks Number of found peaks
627  * @return true if all peaks could be found, false otherwise
628  */
629 bool acc_algorithm_find_peaks(const float *abs_sweep,
630  const uint16_t data_length,
631  const uint32_t *threshold_check,
632  uint16_t *peak_idxs,
633  uint16_t peak_idxs_length,
634  uint16_t *num_peaks);
635 
636 /**
637  * @brief Count points in matrix above threshold row-wise or col-wise
638  *
639  * @param[in] matrix Matrix to check data in
640  * @param[in] rows Number of rows in matrix
641  * @param[in] cols Number of cols in matrix
642  * @param[in] threshold Threshold to check against
643  * @param[out] count Number of elements above threshold, length = rows if axis = 0 or length = cols if axis = 1
644  * @param[in] offset Offset on the selected axis where to start to check threshold
645  * @param[in] threshold_check_length The length of part to check threshold for, row wise if axis = 0 and column wise if axis = 1
646  * @param[in] axis Axis over which to check, must be 0 or 1
647  */
648 void acc_algorithm_count_points_above_threshold(const float *matrix,
649  uint16_t rows,
650  uint16_t cols,
651  const float threshold,
652  uint16_t *count,
653  uint16_t offset,
654  uint16_t threshold_check_length,
655  uint16_t axis);
656 
657 /**
658  * @brief Calculate median of input data
659  *
660  * Median of data of even length is rounded down.
661  * Note that this function sorts inplace and the data array will altered
662  *
663  * @param[in,out] data Array of int16_t values
664  * @param[in] length Length of data
665  * @return The calculated median value
666  */
667 int16_t acc_algorithm_median_i16(int16_t *data, uint16_t length);
668 
669 /**
670  * @brief Calculate median of input data
671  *
672  * Note that this function sorts inplace and the data array will altered
673  *
674  * @param[in, out] data Array of float values
675  * @param[in] length Length of data
676  * @return The calculated median value
677  */
678 float acc_algorithm_median_f32(float *data, uint16_t length);
679 
680 /**
681  * @brief Find max value of input data
682  *
683  * @param[in] data Data to find max value for
684  * @param[in] length Length of data
685  * @return The maximum value
686  */
687 float acc_algorithm_max_f32(const float *data, uint16_t length);
688 
689 /**
690  * @brief Calculate weighted mean
691  *
692  * @param[in] data Data to calculate weighted mean for
693  * @param[in] weights Weighs for each data point in data
694  * @param[in] length Length of data and weights, must be > 0
695  * @return The weighted mean
696  */
697 float acc_algorithm_weighted_mean(const float *data, const float *weights, uint16_t length);
698 
699 /**
700  * @brief Calculate variance
701  *
702  * @param[in] data Data to calculate variance for
703  * @param[in] length Length of data, should be > 1
704  * @return The variance
705  */
706 float acc_algorithm_variance_f32(const float *data, uint16_t length);
707 
708 /**
709  * @brief Calculate standard deviation
710  *
711  * @param[in] data Data to calculate standard deviation for
712  * @param[in] length Length of data, should be > 1
713  * @return The standard deviation
714  */
715 float acc_algorithm_stddev_f32(const float *data, uint16_t length);
716 
717 /**
718  * @brief Clip passed value into the interval [min, max]
719  *
720  * @param[in] value The value to clip
721  * @param[in] min Lower bound of clip interval
722  * @param[in] max Upper bound of clip interval
723  * @return The clipped value
724  */
725 float acc_algorithm_clip_f32(float value, float min, float max);
726 
727 #endif
acc_algorithm_apply_filter_f32
void acc_algorithm_apply_filter_f32(const float *a, const float *filt_data, uint16_t filt_rows, uint16_t filt_cols, const float *b, const float *data, uint16_t data_rows, uint16_t data_cols, float *output, uint16_t output_length)
Apply filter coefficients to filtered data matrix and data matrix.
Definition: acc_algorithm.c:464
acc_algorithm_rfft
void acc_algorithm_rfft(const float *data, uint16_t data_length, uint16_t length_shift, float complex *output)
1D Fast Fourier Transform for real input
Definition: acc_algorithm.c:592
acc_algorithm_exp_smoothing_coefficient
float acc_algorithm_exp_smoothing_coefficient(float fs, float tc)
Calculate exponential smoothing coefficient.
Definition: acc_algorithm.c:694
acc_algorithm_unwrap
void acc_algorithm_unwrap(float *data, uint16_t data_length)
Unwraps a signal by changing elements which have an absolute difference from their predecessor of mor...
Definition: acc_algorithm.c:283
acc_algorithm_welch
void acc_algorithm_welch(const float complex *data, uint16_t data_length, uint16_t segment_length, float complex *data_buffer, float complex *fft_out, float *psd, const float *window, uint16_t length_shift, float fs)
Estimate power spectral density using Welch’s method.
Definition: acc_algorithm.c:879
acc_algorithm_stddev_f32
float acc_algorithm_stddev_f32(const float *data, uint16_t length)
Calculate standard deviation.
Definition: acc_algorithm.c:1426
acc_algorithm_normalize_f32_complex
void acc_algorithm_normalize_f32_complex(float complex *data, uint16_t data_length)
Normalize all elements in an array individually.
Definition: acc_algorithm.c:584
acc_algorithm_get_distance_m
float acc_algorithm_get_distance_m(uint16_t step_length, uint16_t start_point, float base_step_length_m, uint16_t idx)
Calculate distance for a point at an index.
Definition: acc_algorithm.c:1118
acc_int16_complex_t
Data type for interger-based representation of complex numbers.
Definition: acc_definitions_common.h:40
acc_algorithm_double_buffering_frame_filter
void acc_algorithm_double_buffering_frame_filter(acc_int16_complex_t *frame, const uint16_t sweeps_per_frame, const uint16_t num_points, int32_t *work_buffer)
Double buffering frame filter.
Definition: acc_algorithm.c:774
acc_algorithm_interpolate_peaks_equidistant
float acc_algorithm_interpolate_peaks_equidistant(const float *y, float x_start, float x_delta, uint16_t peak_idx)
Interpolate equidistant peaks.
Definition: acc_algorithm.c:330
acc_algorithm_roll_and_push_matrix_f32
void acc_algorithm_roll_and_push_matrix_f32(float *data, uint16_t rows, uint16_t cols, const float *column, bool pos_shift)
Roll row elements and push a new column.
Definition: acc_algorithm.c:169
acc_algorithm_fft_matrix
void acc_algorithm_fft_matrix(const float complex *data, uint16_t rows, uint16_t cols, uint16_t length_shift, float complex *output, uint16_t axis)
1D Fast Fourier Transform for input matrix
Definition: acc_algorithm.c:627
acc_algorithm_roll_and_push
void acc_algorithm_roll_and_push(float *data, uint16_t data_length, float element)
Roll array elements and push new element last.
Definition: acc_algorithm.c:159
acc_algorithm_get_peak_velocity
float acc_algorithm_get_peak_velocity(const float *velocities, const float *energies, const uint16_t *peak_idxs, uint16_t num_peaks, float limit)
Find the velocity of the peak with the largest amplitude, prioritizing peaks with a velocity over the...
Definition: acc_algorithm.c:1009
acc_algorithm_select_profile
acc_config_profile_t acc_algorithm_select_profile(int32_t start_point, float base_step_length)
Select the highest possible profile without interference of direct leakage.
Definition: acc_algorithm.c:1125
acc_algorithm_interpolate_peaks
float acc_algorithm_interpolate_peaks(const float *y, const float *x)
Interpolate peak.
Definition: acc_algorithm.c:322
acc_algorithm_weighted_mean
float acc_algorithm_weighted_mean(const float *data, const float *weights, uint16_t length)
Calculate weighted mean.
Definition: acc_algorithm.c:1375
acc_algorithm_fftfreq
void acc_algorithm_fftfreq(uint16_t n, float d, float *freqs)
Calculate the Fast Fourier Transform sample frequencies.
Definition: acc_algorithm.c:675
acc_algorithm_welch_matrix
void acc_algorithm_welch_matrix(const float complex *data, uint16_t rows, uint16_t cols, uint16_t segment_length, float complex *data_buffer, float complex *fft_out, float *psds, const float *window, uint16_t length_shift, float fs)
Estimate power spectral density (PSD) using Welch’s method along row dimensions.
Definition: acc_algorithm.c:862
acc_algorithm_fftshift_matrix
void acc_algorithm_fftshift_matrix(float *data, uint16_t rows, uint16_t cols)
Shift the zero-frequency component to the center along row dimensions.
Definition: acc_algorithm.c:849
acc_algorithm_find_peaks
bool acc_algorithm_find_peaks(const float *abs_sweep, const uint16_t data_length, const uint32_t *threshold_check, uint16_t *peak_idxs, uint16_t peak_idxs_length, uint16_t *num_peaks)
Find peaks above threshold.
Definition: acc_algorithm.c:1178
acc_algorithm_rfftfreq
void acc_algorithm_rfftfreq(uint16_t n, float d, float *freqs)
Calculate the real Fast Fourier Transform sample frequencies.
Definition: acc_algorithm.c:664
acc_algorithm_butter_lowpass
void acc_algorithm_butter_lowpass(float freq, float fs, float *b, float *a)
Design a 2nd order digital Butterworth lowpass filter.
Definition: acc_algorithm.c:337
acc_algorithm_mean_matrix_i16_complex
void acc_algorithm_mean_matrix_i16_complex(const acc_int16_complex_t *matrix, uint16_t rows, uint16_t cols, float complex *out, uint16_t axis)
Calculate mean array of a matrix.
Definition: acc_algorithm.c:554
acc_algorithm_argmax
uint16_t acc_algorithm_argmax(const float *data, uint16_t data_length)
Find index of largest element in the array.
Definition: acc_algorithm.c:305
acc_config_prf_t
acc_config_prf_t
Pulse Repetition Frequency.
Definition: acc_definitions_a121.h:111
acc_algorithm_hann
void acc_algorithm_hann(uint16_t n, float *window)
Calculate non-symmetrical hann window for a specified number of points.
Definition: acc_algorithm.c:733
acc_algorithm_calculate_mirrored_one_sided_cfar
float acc_algorithm_calculate_mirrored_one_sided_cfar(const float *data, uint16_t data_length, uint16_t middle_idx, uint16_t window_length, uint16_t half_guard_length, float sensitivity, uint16_t idx)
Calculate mirrored one sided CFAR threshold.
Definition: acc_algorithm.c:928
acc_algorithm_rfft_matrix
void acc_algorithm_rfft_matrix(const float *data, uint16_t rows, uint16_t cols, uint16_t length_shift, float complex *output, uint16_t axis)
1D Fast Fourier Transform for real input matrix
Definition: acc_algorithm.c:597
acc_algorithm_get_distance_idx
uint16_t acc_algorithm_get_distance_idx(const float *data, uint16_t cols, uint16_t rows, uint16_t middle_idx, uint16_t half_slow_zone)
Find the index of the distance column containing the largest amplitude, disregarding amplitudes prese...
Definition: acc_algorithm.c:983
acc_algorithm_fft
void acc_algorithm_fft(const float complex *data, uint16_t data_length, uint16_t length_shift, float complex *output)
1D Fast Fourier Transform for complex input
Definition: acc_algorithm.c:622
acc_algorithm_mean_sweep
void acc_algorithm_mean_sweep(const acc_int16_complex_t *frame, uint16_t num_points, uint16_t sweeps_per_frame, uint16_t start_point, uint16_t end_point, float complex *sweep)
Calculate mean sweep of a frame from start_point to end_point.
Definition: acc_algorithm.c:523
acc_algorithm_conj_f32
void acc_algorithm_conj_f32(float complex *data, uint16_t data_length)
Inline calculate conjugate of all elements in an array.
Definition: acc_algorithm.c:576
acc_algorithm_fftfreq_delta
float acc_algorithm_fftfreq_delta(uint16_t n, float d)
Calculate delta between frequency bins in rfft.
Definition: acc_algorithm.c:652
acc_algorithm_select_prf
acc_config_prf_t acc_algorithm_select_prf(int16_t breakpoint, acc_config_profile_t profile, float base_step_length)
Select a suitable PRF given a breakpoint and profile.
Definition: acc_algorithm.c:1144
acc_algorithm_max_f32
float acc_algorithm_max_f32(const float *data, uint16_t length)
Find max value of input data.
Definition: acc_algorithm.c:1360
acc_algorithm_clip_f32
float acc_algorithm_clip_f32(float value, float min, float max)
Clip passed value into the interval [min, max].
Definition: acc_algorithm.c:1431
acc_algorithm_calculate_cfar
float acc_algorithm_calculate_cfar(const float *data, uint16_t data_length, uint16_t window_length, uint16_t half_guard_length, float sensitivity, uint16_t idx)
Calculate CFAR threshold.
Definition: acc_algorithm.c:892
acc_algorithm_median_f32
float acc_algorithm_median_f32(float *data, uint16_t length)
Calculate median of input data.
Definition: acc_algorithm.c:1342
acc_algorithm_lfilter_matrix
void acc_algorithm_lfilter_matrix(const float *b, const float *a, float *data, uint16_t rows, uint16_t cols)
Filter data along row dimension.
Definition: acc_algorithm.c:456
acc_algorithm_median_i16
int16_t acc_algorithm_median_i16(int16_t *data, uint16_t length)
Calculate median of input data.
Definition: acc_algorithm.c:1324
acc_algorithm_roll_and_push_matrix_f32_complex
void acc_algorithm_roll_and_push_matrix_f32_complex(float complex *data, uint16_t rows, uint16_t cols, const float complex *column, bool pos_shift)
Roll row elements and push a new column.
Definition: acc_algorithm.c:203
acc_algorithm_variance_f32
float acc_algorithm_variance_f32(const float *data, uint16_t length)
Calculate variance.
Definition: acc_algorithm.c:1399
acc_algorithm_get_fwhm
float acc_algorithm_get_fwhm(acc_config_profile_t profile)
Get the envelope Full Width Half Maximum in meters given a profile.
Definition: acc_algorithm.c:745
acc_algorithm_fftshift
void acc_algorithm_fftshift(float *data, uint16_t data_length)
Shift the zero-frequency component to the center.
Definition: acc_algorithm.c:857
acc_algorithm_mean_i16_complex
void acc_algorithm_mean_i16_complex(const acc_int16_complex_t *data, uint16_t data_length, float complex *out)
Calculate mean value of an array.
Definition: acc_algorithm.c:549
acc_config_profile_t
acc_config_profile_t
Profile.
Definition: acc_definitions_a121.h:49
acc_definitions_common.h
acc_algorithm_cdiv
float complex acc_algorithm_cdiv(float complex num, float complex denom)
Divide complex number num / denum.
Definition: acc_algorithm.c:708
acc_config.h
acc_algorithm_butter_bandpass
void acc_algorithm_butter_bandpass(float min_freq, float max_freq, float fs, float *b, float *a)
Design a 2nd order digital Butterworth bandpass filter.
Definition: acc_algorithm.c:379
acc_algorithm_lfilter
void acc_algorithm_lfilter(const float *b, const float *a, float *data, uint16_t data_length)
Filter data with a digital filter.
Definition: acc_algorithm.c:446
acc_algorithm_apply_filter_f32_complex
void acc_algorithm_apply_filter_f32_complex(const float *a, const float complex *filt_data, uint16_t filt_rows, uint16_t filt_cols, const float *b, const float complex *data, uint16_t data_rows, uint16_t data_cols, float complex *output, uint16_t output_length)
Apply filter coefficients to filtered data matrix and data matrix.
Definition: acc_algorithm.c:491
acc_algorithm_count_points_above_threshold
void acc_algorithm_count_points_above_threshold(const float *matrix, uint16_t rows, uint16_t cols, const float threshold, uint16_t *count, uint16_t offset, uint16_t threshold_check_length, uint16_t axis)
Count points in matrix above threshold row-wise or col-wise.
Definition: acc_algorithm.c:1281
acc_algorithm_hamming
void acc_algorithm_hamming(uint16_t n, float *window)
Calculate hamming window for a specified number of points.
Definition: acc_algorithm.c:721
acc_algorithm_roll_and_push_mult_matrix_i16_complex
void acc_algorithm_roll_and_push_mult_matrix_i16_complex(acc_int16_complex_t *data, uint16_t data_rows, uint16_t cols, const acc_int16_complex_t *matrix, uint16_t matrix_rows, bool pos_shift)
Roll row elements and push multiple columns.
Definition: acc_algorithm.c:237
acc_algorithm_sum_sweep
void acc_algorithm_sum_sweep(const acc_int16_complex_t *frame, uint16_t num_points, uint16_t sweeps_per_frame, uint16_t start_point, uint16_t end_point, float complex *sum_sweep)
Calculate the sum of all sweeps in a frame, from start_point to end_point.
Definition: acc_algorithm.c:536
acc_definitions_a121.h
acc_algorithm_merge_peaks
bool acc_algorithm_merge_peaks(float max_peak_separation, const float *velocities, const float *energies, const uint16_t *peak_idxs, uint16_t num_peaks, float *merged_velocities, float *merged_energies, uint16_t merged_peaks_length, uint16_t *num_merged_peaks)
Merges peaks.
Definition: acc_algorithm.c:1044