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