//
// Created by SJQ on 2024/3/7.
//

#include "estimator.h"
static const float dt = 1e-3f;
//状态转移矩阵
static float F[4] = {1,dt,
                     0,1};
//控制矩阵
static float B[2] = {0.5f*dt*dt,
                     dt};
//观测矩阵
static float H[4] = {1,0,
                     0,1};
//后验估计协方差初始值
static float P[4] = {1, 0,
                     0, 1};
// R  Q矩阵初始值（其实这里设置多少都无所谓）  需要在调用初始化函数时设置
static float Q[4] = {1e-3f, 0,
                     0, 1e-3f};
static float R[4] = {1000.0f, 0,
                     0, 1000.0f,};
void estimator_init(estimator_t *est ,float process_noise, float measure_noise) {
    Kalman_Filter_Init(&est->EstimateKF_,2,1,2);
    est->EstimateKF_.UseAutoAdjustment = 0;

    for (uint8_t i = 0; i < 4; i += 3)
    {
        // 初始化过程噪声与量测噪声
        Q[i] = process_noise;
        R[i] = measure_noise;
    }
    memcpy(est->EstimateKF_.F_data,F,sizeof(F));
    memcpy(est->EstimateKF_.B_data,B,sizeof(B));
    memcpy(est->EstimateKF_.H_data,H,sizeof(H));
    memcpy(est->EstimateKF_.Q_data,Q,sizeof(Q));
    memcpy(est->EstimateKF_.R_data,R,sizeof(R));

    DWT_GetDeltaT(&est->DWT_CNT_);
}

void estimator_update(estimator_t *est ,float x,float x_dot,float ax)  {
    est->EstimateKF_.MeasuredVector[0] = x;
    est->EstimateKF_.MeasuredVector[1] = x_dot;

    est->EstimateKF_.ControlVector[0] = ax;

    Kalman_Filter_Update(&est->EstimateKF_);

    // 提取估计值

    for (uint8_t i = 0; i < 2; i++)
    {
        if(isnanf(est->EstimateKF_.FilteredValue[i]))
        {
            memset(est->EstimateKF_.xhat_data,0,est->EstimateKF_.xhatSize * 4);
            memset(est->EstimateKF_.xhatminus_data,0,est->EstimateKF_.xhatSize * 4);
            memset(est->EstimateKF_.FilteredValue,0,est->EstimateKF_.xhatSize * 4);
        }
        else
        {
            est->Estimate_X_[i] = est->EstimateKF_.FilteredValue[i];
        }

    }
}