62 lines
1.6 KiB
C++
62 lines
1.6 KiB
C++
|
//
|
|||
|
|
// 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];
|
|||
|
|
}
|