68 lines
2.0 KiB
C
68 lines
2.0 KiB
C
//
|
||
// 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];
|
||
}
|
||
|
||
}
|
||
}
|