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

#include "estimator.h"
static const float dt = 1e-3;
//状态转移矩阵
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.1,
                     0.1, 100};
// P  Q矩阵初始值（其实这里设置多少都无所谓）
static float Q[4] = {0.01, 0,
                     0, 0.01};
static float R[4] = {100000, 0,
                     0, 100000,};
estimator::estimator(float process_noise, float measure_noise) {
    Kalman_Filter_Init(&EstimateKF_,2,1,2);
    EstimateKF_.UseAutoAdjustment = 1;

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

    DWT_GetDeltaT(&DWT_CNT_);
}

void estimator::update(float x,float x_dot,float ax)  {
    EstimateKF_.MeasuredVector[0] = x;
    EstimateKF_.MeasuredVector[1] = x_dot;

    EstimateKF_.ControlVector[0] = ax;

    Kalman_Filter_Update(&EstimateKF_);

    // 提取估计值
    for (uint8_t i = 0; i < 2; i++)
    {
        Estimate_X_[i] = EstimateKF_.FilteredValue[i];
    }
}

void estimator::get_result(float state[2])
{
    state[0] = Estimate_X_[0];
    state[1] = Estimate_X_[1];
}