ref_app_breathing.c

Go to the documentation of this file.

// Copyright (c) Acconeer AB, 2023-2024
// All rights reserved
// This file is subject to the terms and conditions defined in the file
// 'LICENSES/license_acconeer.txt', (BSD 3-Clause License) which is part
// of this source code package.
 
#include <complex.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
 
#include "acc_alg_basic_utils.h"
#include "acc_algorithm.h"
#include "acc_detector_presence.h"
#include "acc_integration.h"
#include "ref_app_breathing.h"
 
#define B_STATIC_LENGTH (3U)
#define A_STATIC_LENGTH (2U)
#define B_ANGLE_LENGTH  (5U)
#define A_ANGLE_LENGTH  (4U)
 
struct ref_app_breathing_handle
{
        acc_detector_presence_handle_t *presence_handle;
 
        float    start_m;
        float    step_length_m;
        int32_t  start_point;
        uint16_t num_points_to_analyze_half_width;
        uint16_t num_points_to_analyze;
        uint16_t end_point;
        float    frame_rate;
        float    lowest_freq;
        float    highest_freq;
        uint16_t use_presence_processor;
        uint16_t distance_determination_count;
        uint16_t time_series_length_s;
        uint16_t time_series_length;
        uint16_t padded_time_series_length_shift;
        uint16_t padded_time_series_length;
        uint16_t num_points;
        uint16_t sweeps_per_frame;
        float    intra_detection_threshold;
 
        ref_app_breathing_app_state_t app_state;
        ref_app_breathing_app_state_t prev_app_state;
 
        float presence_sf;
        float breathing_sf;
 
        float b_static[B_STATIC_LENGTH];
        float a_static[A_STATIC_LENGTH];
        float b_angle[B_ANGLE_LENGTH];
        float a_angle[A_ANGLE_LENGTH];
 
        float complex *mean_sweep;
        float complex *sparse_iq_buffer;
        float complex *filt_sparse_iq_buffer;
        float complex *filt_sparse_iq;
        float         *angle;
        float         *prev_angle;
        float         *lp_filt_ampl;
        float         *unwrapped_angle;
        float         *angle_buffer;
        float         *filt_angle_buffer;
        float         *breathing_motion_buffer;
        float         *hamming_window;
        float         *windowed_breathing_motion_buffer;
        float complex *rfft_output;
        uint16_t       rfft_output_length;
        float         *weighted_psd;
        float          freq_delta;
 
        uint16_t distance_determination_counter;
        bool     presence_init;
        float    presence_distance;
        bool     base_presence_dist;
        float    base_presence_distance;
        float    presence_distance_threshold;
        bool     first;
        uint16_t init_count;
        uint16_t count;
        bool     initialized;
        uint16_t count_limit;
};
 
static bool validate_config(ref_app_breathing_config_t *config);
 
static void determine_state(ref_app_breathing_handle_t *handle, acc_detector_presence_result_t *presence_result);
 
static void update_presence_distance(ref_app_breathing_handle_t *handle, float presence_distance);
 
static bool reinit_breathing(ref_app_breathing_handle_t *handle, uint16_t start_point, uint16_t end_point);
 
static bool perform_action_based_on_state(ref_app_breathing_handle_t *handle, acc_int16_complex_t *frame, ref_app_breathing_result_t *result);
 
static bool process_breathing(ref_app_breathing_handle_t *handle, acc_int16_complex_t *frame, ref_app_breathing_result_t *result);
 
ref_app_breathing_config_t *ref_app_breathing_config_create(void)
{
        ref_app_breathing_config_t *config = acc_integration_mem_alloc(sizeof(*config));
 
        if (config != NULL)
        {
                config->presence_config = acc_detector_presence_config_create();
 
                if (config->presence_config == NULL)
                {
                        ref_app_breathing_config_destroy(config);
                        return NULL;
                }
 
                config->time_series_length_s              = 20U;
                config->lowest_breathing_rate             = 6U;
                config->highest_breathing_rate            = 60U;
                config->num_dists_to_analyze              = 3U;
                config->use_presence_processor            = true;
                config->distance_determination_duration_s = 5U;
 
                acc_detector_presence_config_t *presence_config = config->presence_config;
 
                acc_detector_presence_config_start_set(presence_config, 0.3f);
                acc_detector_presence_config_end_set(presence_config, 1.5f);
                acc_detector_presence_config_hwaas_set(presence_config, 32U);
                acc_detector_presence_config_frame_rate_set(presence_config, 10.0f);
                acc_detector_presence_config_sweeps_per_frame_set(presence_config, 16U);
                acc_detector_presence_config_auto_profile_set(presence_config, false);
                acc_detector_presence_config_profile_set(presence_config, ACC_CONFIG_PROFILE_3);
                acc_detector_presence_config_auto_step_length_set(presence_config, false);
                acc_detector_presence_config_step_length_set(presence_config, 24U);
                acc_detector_presence_config_automatic_subsweeps_set(presence_config, false);
                acc_detector_presence_config_inter_frame_presence_timeout_set(presence_config, 0U);
                acc_detector_presence_config_inter_frame_fast_cutoff_set(presence_config, 20.0f);
                acc_detector_presence_config_intra_detection_threshold_set(presence_config, 6.0f);
                acc_detector_presence_config_intra_output_time_const_set(presence_config, 0.5f);
                acc_detector_presence_config_inter_output_time_const_set(presence_config, 5.0f);
        }
 
        return config;
}
 
void ref_app_breathing_config_destroy(ref_app_breathing_config_t *config)
{
        if (config != NULL)
        {
                if (config->presence_config != NULL)
                {
                        acc_detector_presence_config_destroy(config->presence_config);
                }
 
                acc_integration_mem_free(config);
        }
}
 
ref_app_breathing_handle_t *ref_app_breathing_create(ref_app_breathing_config_t *config)
{
        if (!validate_config(config))
        {
                return NULL;
        }
 
        ref_app_breathing_handle_t *handle = acc_integration_mem_calloc(1, sizeof(*handle));
 
        if (handle != NULL)
        {
                handle->frame_rate                = acc_detector_presence_config_frame_rate_get(config->presence_config);
                handle->sweeps_per_frame          = acc_detector_presence_config_sweeps_per_frame_get(config->presence_config);
                handle->intra_detection_threshold = acc_detector_presence_config_intra_detection_threshold_get(config->presence_config);
 
                acc_detector_presence_metadata_t presence_metadata;
 
                handle->presence_handle = acc_detector_presence_create(config->presence_config, &presence_metadata);
 
                if (handle->presence_handle == NULL)
                {
                        ref_app_breathing_destroy(handle);
                        return NULL;
                }
 
                handle->start_m       = presence_metadata.start_m;
                handle->step_length_m = presence_metadata.step_length_m;
                handle->num_points    = presence_metadata.num_points;
 
                handle->lowest_freq                      = (float)config->lowest_breathing_rate / 60.0f;
                handle->highest_freq                     = (float)config->highest_breathing_rate / 60.0f;
                handle->use_presence_processor           = config->use_presence_processor;
                handle->time_series_length_s             = config->time_series_length_s;
                handle->distance_determination_count     = config->distance_determination_duration_s * handle->frame_rate;
                handle->num_points_to_analyze_half_width = config->num_dists_to_analyze / 2U;
                handle->num_points_to_analyze = config->use_presence_processor ? handle->num_points_to_analyze_half_width * 2U + 1U : handle->num_points;
 
                handle->time_series_length              = handle->time_series_length_s * handle->frame_rate;
                handle->padded_time_series_length_shift = 0U;
                handle->padded_time_series_length       = 1U << handle->padded_time_series_length_shift;
 
                while (handle->padded_time_series_length < handle->time_series_length)
                {
                        handle->padded_time_series_length_shift++;
                        handle->padded_time_series_length = 1U << handle->padded_time_series_length_shift;
                }
 
                handle->rfft_output_length             = (handle->padded_time_series_length / 2U) + 1U;
                handle->distance_determination_counter = 0U;
                handle->presence_init                  = false;
                handle->presence_distance              = 0.0f;
                handle->base_presence_dist             = false;
                handle->base_presence_distance         = 0.0f;
                handle->presence_distance_threshold    = acc_algorithm_get_fwhm(acc_detector_presence_config_profile_get(config->presence_config)) * 2.0f;
                handle->first                          = true;
                handle->init_count                     = 0U;
                handle->count                          = 0U;
                handle->initialized                    = false;
                handle->presence_sf  = acc_algorithm_exp_smoothing_coefficient(handle->frame_rate, (float)config->distance_determination_duration_s / 4.0f);
                handle->breathing_sf = acc_algorithm_exp_smoothing_coefficient(handle->frame_rate, handle->time_series_length_s / 2.0f);
 
                handle->app_state      = REF_APP_BREATHING_APP_STATE_INIT;
                handle->prev_app_state = REF_APP_BREATHING_APP_STATE_INIT;
 
                handle->count_limit = handle->time_series_length / 2U;
 
                acc_algorithm_butter_lowpass(handle->lowest_freq, handle->frame_rate, handle->b_static, handle->a_static);
                acc_algorithm_butter_bandpass(handle->lowest_freq, handle->highest_freq, handle->frame_rate, handle->b_angle, handle->a_angle);
 
                handle->mean_sweep       = acc_integration_mem_alloc(handle->num_points_to_analyze * sizeof(*handle->mean_sweep));
                handle->filt_sparse_iq   = acc_integration_mem_alloc(handle->num_points_to_analyze * sizeof(*handle->filt_sparse_iq));
                handle->sparse_iq_buffer = acc_integration_mem_alloc(B_STATIC_LENGTH * handle->num_points_to_analyze * sizeof(*handle->sparse_iq_buffer));
                handle->filt_sparse_iq_buffer =
                    acc_integration_mem_alloc(A_STATIC_LENGTH * handle->num_points_to_analyze * sizeof(*handle->filt_sparse_iq_buffer));
                handle->angle             = acc_integration_mem_alloc(handle->num_points_to_analyze * sizeof(*handle->angle));
                handle->prev_angle        = acc_integration_mem_alloc(handle->num_points_to_analyze * sizeof(*handle->prev_angle));
                handle->lp_filt_ampl      = acc_integration_mem_alloc(handle->num_points_to_analyze * sizeof(*handle->lp_filt_ampl));
                handle->unwrapped_angle   = acc_integration_mem_alloc(handle->num_points_to_analyze * sizeof(*handle->unwrapped_angle));
                handle->angle_buffer      = acc_integration_mem_alloc(B_ANGLE_LENGTH * handle->num_points_to_analyze * sizeof(*handle->angle_buffer));
                handle->filt_angle_buffer = acc_integration_mem_alloc(A_ANGLE_LENGTH * handle->num_points_to_analyze * sizeof(*handle->filt_angle_buffer));
                handle->breathing_motion_buffer =
                    acc_integration_mem_alloc(handle->time_series_length * handle->num_points_to_analyze * sizeof(*handle->breathing_motion_buffer));
                handle->hamming_window = acc_integration_mem_alloc(handle->time_series_length * sizeof(*handle->hamming_window));
                handle->windowed_breathing_motion_buffer =
                    acc_integration_mem_alloc(handle->time_series_length * handle->num_points_to_analyze * sizeof(*handle->windowed_breathing_motion_buffer));
                handle->rfft_output  = acc_integration_mem_alloc(handle->rfft_output_length * handle->num_points_to_analyze * sizeof(*handle->rfft_output));
                handle->weighted_psd = acc_integration_mem_alloc(handle->rfft_output_length * sizeof(*handle->weighted_psd));
 
                bool status = handle->mean_sweep != NULL && handle->filt_sparse_iq != NULL && handle->sparse_iq_buffer != NULL &&
                              handle->filt_sparse_iq_buffer != NULL && handle->angle != NULL && handle->prev_angle != NULL && handle->lp_filt_ampl != NULL &&
                              handle->unwrapped_angle != NULL && handle->angle_buffer != NULL && handle->filt_angle_buffer != NULL &&
                              handle->breathing_motion_buffer != NULL && handle->hamming_window != NULL && handle->windowed_breathing_motion_buffer != NULL &&
                              handle->rfft_output != NULL && handle->weighted_psd != NULL;
 
                if (status)
                {
                        handle->freq_delta = acc_algorithm_fftfreq_delta(handle->padded_time_series_length, 1.0f / handle->frame_rate);
                        acc_algorithm_hamming(handle->time_series_length, handle->hamming_window);
                }
                else
                {
                        ref_app_breathing_destroy(handle);
                        return NULL;
                }
        }
 
        return handle;
}
 
void ref_app_breathing_destroy(ref_app_breathing_handle_t *handle)
{
        if (handle != NULL)
        {
                if (handle->presence_handle != NULL)
                {
                        acc_detector_presence_destroy(handle->presence_handle);
                }
 
                if (handle->mean_sweep != NULL)
                {
                        acc_integration_mem_free(handle->mean_sweep);
                }
 
                if (handle->filt_sparse_iq != NULL)
                {
                        acc_integration_mem_free(handle->filt_sparse_iq);
                }
 
                if (handle->sparse_iq_buffer != NULL)
                {
                        acc_integration_mem_free(handle->sparse_iq_buffer);
                }
 
                if (handle->filt_sparse_iq_buffer != NULL)
                {
                        acc_integration_mem_free(handle->filt_sparse_iq_buffer);
                }
 
                if (handle->angle != NULL)
                {
                        acc_integration_mem_free(handle->angle);
                }
 
                if (handle->prev_angle != NULL)
                {
                        acc_integration_mem_free(handle->prev_angle);
                }
 
                if (handle->lp_filt_ampl != NULL)
                {
                        acc_integration_mem_free(handle->lp_filt_ampl);
                }
 
                if (handle->unwrapped_angle != NULL)
                {
                        acc_integration_mem_free(handle->unwrapped_angle);
                }
 
                if (handle->angle_buffer != NULL)
                {
                        acc_integration_mem_free(handle->angle_buffer);
                }
 
                if (handle->filt_angle_buffer != NULL)
                {
                        acc_integration_mem_free(handle->filt_angle_buffer);
                }
 
                if (handle->breathing_motion_buffer != NULL)
                {
                        acc_integration_mem_free(handle->breathing_motion_buffer);
                }
 
                if (handle->hamming_window != NULL)
                {
                        acc_integration_mem_free(handle->hamming_window);
                }
 
                if (handle->windowed_breathing_motion_buffer != NULL)
                {
                        acc_integration_mem_free(handle->windowed_breathing_motion_buffer);
                }
 
                if (handle->rfft_output != NULL)
                {
                        acc_integration_mem_free(handle->rfft_output);
                }
 
                if (handle->weighted_psd != NULL)
                {
                        acc_integration_mem_free(handle->weighted_psd);
                }
 
                acc_integration_mem_free(handle);
        }
}
 
bool ref_app_breathing_get_buffer_size(ref_app_breathing_handle_t *handle, uint32_t *buffer_size)
{
        return acc_detector_presence_get_buffer_size(handle->presence_handle, buffer_size);
}
 
bool ref_app_breathing_prepare(ref_app_breathing_handle_t *handle,
                               ref_app_breathing_config_t *config,
                               acc_sensor_t               *sensor,
                               const acc_cal_result_t     *sensor_cal_result,
                               void                       *buffer,
                               uint32_t                    buffer_size)
{
        return acc_detector_presence_prepare(handle->presence_handle, config->presence_config, sensor, sensor_cal_result, buffer, buffer_size);
}
 
bool ref_app_breathing_process(ref_app_breathing_handle_t *handle, void *buffer, ref_app_breathing_result_t *result)
{
        bool status = acc_detector_presence_process(handle->presence_handle, buffer, &result->presence_result);
 
        if (status)
        {
                if (result->presence_result.processing_result.calibration_needed)
                {
                        handle->base_presence_dist     = false;
                        handle->base_presence_distance = 0.0f;
                }
                else
                {
                        determine_state(handle, &result->presence_result);
 
                        update_presence_distance(handle, result->presence_result.presence_distance);
 
                        status = perform_action_based_on_state(handle, result->presence_result.processing_result.frame, result);
                }
        }
 
        if (status)
        {
                result->app_state = handle->app_state;
 
                handle->prev_app_state = handle->app_state;
        }
 
        return status;
}
 
static bool validate_config(ref_app_breathing_config_t *config)
{
        float frame_rate = acc_detector_presence_config_frame_rate_get(config->presence_config);
 
        bool status = true;
 
        if (frame_rate == 0.0f)
        {
                printf("Frame rate must be set, i.e. > 0.0\n");
                status = false;
        }
 
        if (config->lowest_breathing_rate >= config->highest_breathing_rate)
        {
                printf("Lowest breathing rate must be lower than highest breathing rate\n");
                status = false;
        }
 
        if (config->num_dists_to_analyze < 1U)
        {
                printf("Number of distances to analyze must be higher than 1");
                status = false;
        }
 
        return status;
}
 
static void determine_state(ref_app_breathing_handle_t *handle, acc_detector_presence_result_t *presence_result)
{
        if (!presence_result->presence_detected)
        {
                handle->app_state = REF_APP_BREATHING_APP_STATE_NO_PRESENCE;
        }
        else if (handle->intra_detection_threshold < presence_result->intra_presence_score)
        {
                handle->app_state = REF_APP_BREATHING_APP_STATE_INTRA_PRESENCE;
        }
        else if (!handle->base_presence_dist && handle->use_presence_processor)
        {
                handle->app_state = REF_APP_BREATHING_APP_STATE_DETERMINE_DISTANCE;
        }
        else if (!handle->use_presence_processor || handle->distance_determination_count <= handle->distance_determination_counter)
        {
                handle->app_state = REF_APP_BREATHING_APP_STATE_ESTIMATE_BREATHING_RATE;
        }
        else
        {
                // Do nothing
        }
}
 
static void update_presence_distance(ref_app_breathing_handle_t *handle, float presence_distance)
{
        if (!handle->presence_init)
        {
                handle->presence_init     = true;
                handle->presence_distance = presence_distance;
        }
 
        handle->presence_distance = handle->presence_distance * handle->presence_sf + presence_distance * (1.0f - handle->presence_sf);
 
        if (handle->base_presence_dist && handle->presence_distance_threshold < fabsf(handle->base_presence_distance - handle->presence_distance))
        {
                handle->base_presence_dist     = false;
                handle->base_presence_distance = 0.0f;
        }
}
 
static bool reinit_breathing(ref_app_breathing_handle_t *handle, uint16_t start_point, uint16_t end_point)
{
        handle->start_point           = start_point;
        handle->end_point             = end_point;
        handle->num_points_to_analyze = end_point - start_point;
 
        handle->first       = true;
        handle->init_count  = 0U;
        handle->count       = 0U;
        handle->initialized = false;
 
        memset(handle->sparse_iq_buffer, 0, B_STATIC_LENGTH * handle->num_points_to_analyze * sizeof(*handle->sparse_iq_buffer));
        memset(handle->filt_sparse_iq_buffer, 0, A_STATIC_LENGTH * handle->num_points_to_analyze * sizeof(*handle->filt_sparse_iq_buffer));
        memset(handle->prev_angle, 0, handle->num_points_to_analyze * sizeof(*handle->prev_angle));
        memset(handle->lp_filt_ampl, 0, handle->num_points_to_analyze * sizeof(*handle->lp_filt_ampl));
        memset(handle->unwrapped_angle, 0, handle->num_points_to_analyze * sizeof(*handle->unwrapped_angle));
        memset(handle->angle_buffer, 0, B_ANGLE_LENGTH * handle->num_points_to_analyze * sizeof(*handle->angle_buffer));
        memset(handle->filt_angle_buffer, 0, A_ANGLE_LENGTH * handle->num_points_to_analyze * sizeof(*handle->filt_angle_buffer));
        memset(handle->breathing_motion_buffer, 0, handle->time_series_length * handle->num_points_to_analyze * sizeof(*handle->breathing_motion_buffer));
 
        return true;
}
 
static bool perform_action_based_on_state(ref_app_breathing_handle_t *handle, acc_int16_complex_t *frame, ref_app_breathing_result_t *result)
{
        bool status = true;
 
        result->result_ready = false;
 
        switch (handle->app_state)
        {
                case REF_APP_BREATHING_APP_STATE_INIT:
                        // Do nothing
                        break;
                // No presence and intra presence require the same action
                case REF_APP_BREATHING_APP_STATE_NO_PRESENCE:
                case REF_APP_BREATHING_APP_STATE_INTRA_PRESENCE:
                        handle->base_presence_dist     = false;
                        handle->base_presence_distance = 0.0f;
                        break;
                case REF_APP_BREATHING_APP_STATE_DETERMINE_DISTANCE:
                        if (handle->app_state != handle->prev_app_state)
                        {
                                handle->distance_determination_counter = 0U;
                        }
                        else
                        {
                                handle->distance_determination_counter++;
                                handle->base_presence_dist     = true;
                                handle->base_presence_distance = handle->presence_distance;
                        }
 
                        break;
                case REF_APP_BREATHING_APP_STATE_ESTIMATE_BREATHING_RATE:
                        if (handle->app_state != handle->prev_app_state)
                        {
                                uint16_t start_p;
                                uint16_t end_p;
 
                                if (handle->use_presence_processor)
                                {
                                        uint16_t center_idx = (uint16_t)(((handle->base_presence_distance - handle->start_m) / handle->step_length_m) + 0.5f);
                                        start_p             = center_idx >= handle->num_points_to_analyze_half_width
                                                                  ? (uint16_t)(center_idx - handle->num_points_to_analyze_half_width)
                                                                  : 0U;
                                        end_p               = (center_idx + handle->num_points_to_analyze_half_width + 1U) <= handle->num_points
                                                                  ? (center_idx + handle->num_points_to_analyze_half_width + 1U)
                                                                  : handle->num_points;
                                }
                                else
                                {
                                        start_p = 0U;
                                        end_p   = handle->num_points;
                                }
 
                                reinit_breathing(handle, start_p, end_p);
                        }
 
                        status = process_breathing(handle, frame, result);
                        break;
                default:
                        // Should never happen
                        break;
        }
 
        return status;
}
 
static bool process_breathing(ref_app_breathing_handle_t *handle, acc_int16_complex_t *frame, ref_app_breathing_result_t *result)
{
        acc_algorithm_mean_sweep(frame, handle->num_points, handle->sweeps_per_frame, handle->start_point, handle->end_point, handle->mean_sweep);
 
        acc_algorithm_roll_and_push_matrix_f32_complex(handle->sparse_iq_buffer,
                                                       B_STATIC_LENGTH,
                                                       handle->num_points_to_analyze,
                                                       handle->mean_sweep,
                                                       true);
 
        acc_algorithm_apply_filter_f32_complex(handle->a_static,
                                               handle->filt_sparse_iq_buffer,
                                               A_STATIC_LENGTH,
                                               handle->num_points_to_analyze,
                                               handle->b_static,
                                               handle->sparse_iq_buffer,
                                               B_STATIC_LENGTH,
                                               handle->num_points_to_analyze,
                                               handle->filt_sparse_iq,
                                               handle->num_points_to_analyze);
 
        acc_algorithm_roll_and_push_matrix_f32_complex(handle->filt_sparse_iq_buffer,
                                                       A_STATIC_LENGTH,
                                                       handle->num_points_to_analyze,
                                                       handle->filt_sparse_iq,
                                                       true);
 
        for (uint16_t i = 0U; i < handle->num_points_to_analyze; i++)
        {
                handle->mean_sweep[i] = handle->mean_sweep[i] - handle->filt_sparse_iq[i];
                handle->angle[i]      = cargf(handle->mean_sweep[i]);
        }
 
        if (handle->first)
        {
                for (uint16_t i = 0U; i < handle->num_points_to_analyze; i++)
                {
                        handle->prev_angle[i]   = handle->angle[i];
                        handle->lp_filt_ampl[i] = cabsf(handle->mean_sweep[i]);
                }
 
                handle->first = false;
        }
 
        for (uint16_t i = 0U; i < handle->num_points_to_analyze; i++)
        {
                handle->lp_filt_ampl[i] = handle->breathing_sf * handle->lp_filt_ampl[i] + (1.0f - handle->breathing_sf) * cabsf(handle->mean_sweep[i]);
        }
 
        for (uint16_t i = 0U; i < handle->num_points_to_analyze; i++)
        {
                float angle_diff      = handle->angle[i] - handle->prev_angle[i];
                handle->prev_angle[i] = handle->angle[i];
 
                if ((float)M_PI < angle_diff)
                {
                        angle_diff -= 2.0f * (float)M_PI;
                }
                else if (angle_diff < -(float)M_PI)
                {
                        angle_diff += 2.0f * (float)M_PI;
                }
 
                handle->unwrapped_angle[i] += angle_diff;
        }
 
        acc_algorithm_roll_and_push_matrix_f32(handle->angle_buffer, B_ANGLE_LENGTH, handle->num_points_to_analyze, handle->unwrapped_angle, true);
 
        acc_algorithm_apply_filter_f32(handle->a_angle,
                                       handle->filt_angle_buffer,
                                       A_ANGLE_LENGTH,
                                       handle->num_points_to_analyze,
                                       handle->b_angle,
                                       handle->angle_buffer,
                                       B_ANGLE_LENGTH,
                                       handle->num_points_to_analyze,
                                       handle->angle,
                                       handle->num_points_to_analyze);
 
        acc_algorithm_roll_and_push_matrix_f32(handle->filt_angle_buffer, A_ANGLE_LENGTH, handle->num_points_to_analyze, handle->angle, true);
 
        acc_algorithm_roll_and_push_matrix_f32(handle->breathing_motion_buffer,
                                               handle->time_series_length,
                                               handle->num_points_to_analyze,
                                               handle->angle,
                                               false);
 
        if (handle->init_count > handle->time_series_length)
        {
                handle->initialized = true;
        }
        else
        {
                handle->init_count++;
        }
 
        if (handle->time_series_length - handle->count_limit <= handle->count)
        {
                handle->count = 0;
 
                if (handle->initialized)
                {
                        float lp_filt_ampl_sum = 0U;
 
                        for (uint16_t r = 0U; r < handle->time_series_length; r++)
                        {
                                for (uint16_t c = 0U; c < handle->num_points_to_analyze; c++)
                                {
                                        handle->windowed_breathing_motion_buffer[r * handle->num_points_to_analyze + c] =
                                            handle->breathing_motion_buffer[r * handle->num_points_to_analyze + c] * handle->hamming_window[r];
                                        lp_filt_ampl_sum += handle->lp_filt_ampl[c];
                                }
                        }
 
                        acc_algorithm_rfft_matrix(handle->windowed_breathing_motion_buffer,
                                                  handle->time_series_length,
                                                  handle->num_points_to_analyze,
                                                  handle->padded_time_series_length_shift,
                                                  handle->rfft_output,
                                                  0U);
 
                        for (uint16_t r = 0U; r < handle->rfft_output_length; r++)
                        {
                                float sum_psd = 0U;
 
                                for (uint16_t c = 0U; c < handle->num_points_to_analyze; c++)
                                {
                                        sum_psd += cabsf(handle->rfft_output[r * handle->num_points_to_analyze + c]) * handle->lp_filt_ampl[c];
                                }
 
                                handle->weighted_psd[r] = sum_psd / lp_filt_ampl_sum;
                        }
 
                        uint16_t peak_loc = acc_algorithm_argmax(handle->weighted_psd, handle->rfft_output_length);
 
                        if (peak_loc > 0U)
                        {
                                float freq             = acc_algorithm_interpolate_peaks_equidistant(handle->weighted_psd, 0.0f, handle->freq_delta, peak_loc);
                                result->result_ready   = true;
                                result->breathing_rate = freq * 60.0f;
                        }
                }
        }
        else
        {
                handle->count++;
        }
 
        return true;
}