// 文件编码: GB18030
/*
 * filter.c
 *
 *  Created on: 2025年4月6日
 *      Author: LHYe200
 */

#include "filter.h"
#include "math.h"
#include <stdlib.h>
#include <string.h>

/*******************************************************************************
 * @fn Init_lowPass_alpha
 * @brief 初始化低通滤波器滤波系数
 * @param filter 滤波器
 * @param ts 采用周期 单位s
 * @return fc 截至频率 单位hz
 ******************************************************************************/
void Init_lowPass_alpha(low_pass_filter_t* const filter,const float ts, const float fc)
{
    float b=2*M_PI*fc*ts;
    filter->ts=ts;
    filter->fc=fc;
    filter->lastYn=0;
    filter->alpha=b/(b+1);
}

/*******************************************************************************
 * @fn Low_pass_filter
 * @brief 低通滤波函数
 * @param data 采样数据
 * @return 滤波结果
 ******************************************************************************/
float Low_pass_filter(low_pass_filter_t* const filter, float data)
{
    float tem=filter->lastYn+(filter->alpha*(data-filter->lastYn));
    filter->lastYn=tem;
    return tem;
}

/*******************************************************************************
 * @fn Init_hightPass_alpha
 * @brief 初始高通滤波器滤波系数
 * @param filter 滤波器
 * @param ts 采用周期 单位s
 * @return fc 截至频率 单位hz
 ******************************************************************************/
void Init_hightPass_alpha(hight_pass_filter_t* const filter,const float ts, const float fc)
{
    float b=2*M_PI*fc*ts;
    filter->ts=ts;
    filter->fc=fc;
    filter->lastYn=0;
    filter->lastXn=0;
    filter->alpha=1/(1+b);
}

/*******************************************************************************
 * @fn Hight_pass_filter
 * @brief 高通滤波函数
 * @param data 采样数据
 * @return 滤波结果
 ******************************************************************************/
float Hight_pass_filter(hight_pass_filter_t* const filter, float data)
{
    float tem=((filter->alpha)*(filter->lastYn))+((filter->alpha)*(data-(filter->lastXn)));
    filter->lastYn=tem;
    filter->lastXn=data;
    return tem;
}

/*******************************************************************************
 * @fn Moving_Average_filter_init
 * @brief 初始化滑动平均滤波器
 * @param filter 滤波器结构体指针
 * @param size 窗口大小
 * @return void
 ******************************************************************************/
void Moving_Average_filter_init(moving_average_filter_t* const filter, int size)
{
    filter->size = size;
    filter->avg = 0.0f;
    filter->window = (float*)malloc(size * sizeof(float));
    memset(filter->window, 0, size * sizeof(float));
}

/*******************************************************************************
 * @fn Moving_Average_filter
 * @brief 滑动平均滤波器实现
 * @param data 输入数据数组
 * @param size 数据数组的大小
 * @return 返回滑动平均值
 * @note 适用于平滑随机噪声信号。
 ******************************************************************************/
float Moving_Average_filter(moving_average_filter_t* const filter, float newData)
{
    float sum = 0.0f;
    // 移动窗口
    for(int i = 0; i < filter->size - 1; i++)
    {
        filter->window[i] = filter->window[i + 1];
        sum += filter->window[i];
    }
    filter->window[filter->size - 1] = newData;
    sum += filter->window[filter->size - 1];
    // 计算平均值
    filter->avg = sum / filter->size;
    return filter->avg;
}


/*******************************************************************************
 * @fn Kalman_filter_init
 * @brief 初始化卡尔曼滤波器状态与参数
 * @param filter 卡尔曼滤波器结构体指针
 * @param q 过程噪声方差（过程噪声协方差Q）
 * @param r 测量噪声方差（测量噪声协方差R）
 * @param kGain 初始卡尔曼增益（可设为0或估计值）
 * @return void
 * @note prevData 初始为0；p 初始协方差设为1。本函数仅做初始状态配置，适用于一维单变量卡尔曼滤波结构。
 ******************************************************************************/
void Kalman_filter_init(Kalman_filter_t* const filter, const float q, const float r)
{
    filter->prevData = 0;
    filter->p = 1;
    filter->q = q;
    filter->r = r;
    filter->kGain = 0;
}

/*******************************************************************************
 * @fn Kalman_filter
 * @brief 一维卡尔曼滤波器实现（单变量增量式更新）
 * @param filter 卡尔曼滤波器结构体指针，包含状态 prevData，协方差 p，噪声q/r 和当前增益 kGain
 * @param inData 本次测量输入值
 * @return 返回滤波后的估计值（浮点型）
 * @note 算法步骤：
 *       1) 预测协方差 p <- p + q
 *       2) 计算卡尔曼增益 k <- p / (p + r)
 *       3) 使用增益修正估计值 x <- x_prev + k*(z - x_prev)
 *       4) 更新协方差 p <- (1 - k) * p
 *       5) 保存本次估计为 prevData 并返回
 *       该实现适用于一维信号的在线滤波，未包含状态转移矩阵或控制输入。
 ******************************************************************************/
float Kalman_filter(Kalman_filter_t* const filter, float inData)
{
    filter->p = filter->p + filter->q;
    filter->kGain = filter->p / ( filter->p + filter->r );                                     //计算卡尔曼增益
    inData = filter->prevData + ( filter->kGain * ( inData - filter->prevData ) );     //计算本次滤波估计值
    filter->p = ( 1 - filter->kGain ) * filter->p;                                     //更新测量方差
    filter->prevData = inData;
    return inData;                                      //返回滤波值
}


/*******************************************************************************
 * @fn Unscented_Kalman_filter_init
 * @brief 初始化无迹卡尔曼滤波器状态与参数
 * @param filter 无迹卡尔曼滤波器结构体指针
 * @param q 过程噪声协方差
 * @param r 测量噪声协方差
 * @return void
 * @note prevData 初始为0；p 初始协方差设为1。
 ******************************************************************************/
void Unscented_Kalman_filter_init(Kalman_filter_t* const filter, const float q, const float r)
{
    filter->prevData = 0;
    filter->p = 1;
    filter->q = q;
    filter->r = r;
    filter->kGain = 0;
}

/*******************************************************************************
 * @fn Unscented_Kalman_filter
 * @brief 一维无迹卡尔曼滤波器实现
 * @param filter 卡尔曼滤波器结构体指针，包含状态、协方差、噪声参数等
 * @param inData 本次测量输入值
 * @return 返回滤波后的估计值（浮点型）
 * @note 使用无迹变换处理非线性系统，适合复杂非线性信号。
 ******************************************************************************/
float Unscented_Kalman_filter(Kalman_filter_t* const filter, float inData)
{
    // 无迹变换步骤
    float sigmaPoints[3];
    sigmaPoints[0] = filter->prevData;
    sigmaPoints[1] = filter->prevData + sqrtf(filter->p + filter->q);
    sigmaPoints[2] = filter->prevData - sqrtf(filter->p + filter->q);

    // 预测均值
    float predictedMean = (sigmaPoints[0] + sigmaPoints[1] + sigmaPoints[2]) / 3.0;

    // 预测协方差
    float predictedCov = filter->q + ((sigmaPoints[1] - predictedMean) * (sigmaPoints[1] - predictedMean) +
                                      (sigmaPoints[2] - predictedMean) * (sigmaPoints[2] - predictedMean)) / 2.0;

    // 更新卡尔曼增益
    filter->kGain = predictedCov / (predictedCov + filter->r);

    // 更新估计值
    float updatedMean = predictedMean + filter->kGain * (inData - predictedMean);

    // 更新协方差
    filter->p = (1 - filter->kGain) * predictedCov;

    // 保存状态
    filter->prevData = updatedMean;

    return updatedMean;
}



/*******************************************************************************
 * @fn Adaptive_Kalman_filter_init
 * @brief 初始化自适应卡尔曼滤波器状态与参数
 * @param filter 自适应卡尔曼滤波器结构体指针
 * @param q 初始过程噪声协方差
 * @param r 测量噪声协方差
 * @return void
 * @note prevData 初始为0；p 初始协方差设为1。
 ******************************************************************************/
void Adaptive_Kalman_filter_init(Kalman_filter_t* const filter, const float q, const float r)
{
    filter->prevData = 0;
    filter->p = 1;
    filter->q = q;
    filter->r = r;
    filter->kGain = 0;
}

/*******************************************************************************
 * @fn Adaptive_Kalman_filter
 * @brief 一维自适应卡尔曼滤波器实现
 * @param filter 卡尔曼滤波器结构体指针，包含状态、协方差、噪声参数等
 * @param inData 本次测量输入值
 * @return 返回滤波后的估计值（浮点型）
 * @note 动态调整噪声协方差，适合噪声特性变化的信号。
 ******************************************************************************/
float Adaptive_Kalman_filter(Kalman_filter_t* const filter, float inData)
{
    // 更新过程噪声协方差 Q 和测量噪声协方差 R
    float innovation = inData - filter->prevData;
    float alpha = fabsf(innovation) > 1.0f ? 0.2f : 0.8f; // 根据创新调整 ALPHA
    filter->q = alpha * filter->q + (1 - alpha) * (innovation * innovation);

    // 预测协方差
    filter->p = filter->p + filter->q;

    // 更新卡尔曼增益
    filter->kGain = filter->p / (filter->p + filter->r);

    // 更新估计值
    float updatedData = filter->prevData + filter->kGain * innovation;

    // 更新协方差
    filter->p = (1 - filter->kGain) * filter->p;

    // 保存状态
    filter->prevData = updatedData;

    return updatedData;
}
