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

#include "estimator.h"
static const float dt = 1e-3f;
//状态转移矩阵
static float F[4] = {0,1,
                     1,0};
//控制矩阵
static float B[2] = {dt,
                     1};
//观测矩阵
static float H[4] = {1,0,
                     0,1};
//后验估计协方差初始值
static float P[4] = {100, 0.1f,
                     0.1f, 100};
// R  Q矩阵初始值（其实这里设置多少都无所谓）
static float Q[4] = {1.0f, 0,
                     0, 1.0f};
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++)
    {
        est->Estimate_X_[i] = est->EstimateKF_.FilteredValue[i];
    }
}