92 lines
3.4 KiB
C
92 lines
3.4 KiB
C
//
|
||
// Created by SJQ on 2024/1/13.
|
||
//
|
||
|
||
#include "observer.h"
|
||
#include "bsp_dwt.h"
|
||
#include "arm_math.h"
|
||
|
||
static const float dt = 1e-3;
|
||
static const float p_A[6][3] = {
|
||
{110.149443140716,-209.796047546046, 108.212820026855},
|
||
{6.56756328149907,-12.5089040777703,6.45209383844021},
|
||
{20.8432584790045,-14.5681682729471,-0.265391946757807},
|
||
{1.24276088149285,-0.868614169078260,0.164688375466239},
|
||
{-121.880643460273,85.1871469584697,1.55187545520287},
|
||
{-7.26702574149722,5.07920841420309,16.7402602760541}
|
||
};
|
||
|
||
static const float p_B[6][3] = {
|
||
{-171.924494208671,125.455249123645,-27.5944606199982},
|
||
{169.057788940511,-119.995181259560,24.7781571289566},
|
||
{-0.576505386985648,0.0174968125883799,0.781810074568448},
|
||
{0.0340470941093140,0.361648497135550,-0.267186973741535},
|
||
{3.37110666237256,-0.102312350965865,-1.14202204587003},
|
||
{-0.199089875614915,-2.11473419963208,6.60130194247413}
|
||
};
|
||
|
||
|
||
float gEstimateKF_F[36] = {1, dt, 0, 0, 0,0,
|
||
0, 1, 0, 0, 0,0,
|
||
0, 0, 1, dt, 0,0,
|
||
0, 0, 0, 1, 0,0,
|
||
0, 0, 0, 0, 1,dt,
|
||
0, 0, 0, 0, 0,1}; // 状态转移矩阵其余项在滤波器更新时更新
|
||
float gEstimateKF_B[12] = {0, 0, 0,
|
||
0, 0, 0,
|
||
0, 0, 0,
|
||
0, 0, 0}; // 控制矩阵其余项在滤波器更新时更新
|
||
|
||
const float gEstimateKF_H[36] = {1, 0, 0, 0, 0,0,
|
||
0, 1, 0, 0, 0,0,
|
||
0, 0, 1, 0, 0,0,
|
||
0, 0, 0, 1, 0,0,
|
||
0, 0, 0, 0, 1,0,
|
||
0, 0, 0, 0, 0,1}; // 由于不需要异步量测自适应，这里直接设置矩阵H为常量
|
||
|
||
|
||
void Observer_Init(BalanceObserver* observer)
|
||
{
|
||
KalmanFilter_t *kf = &observer->stateEstimateKF;
|
||
Kalman_Filter_Init(kf,6,2,6);
|
||
|
||
//暂时只使用状态预测一个步骤
|
||
kf->UseAutoAdjustment = 0;
|
||
kf->SkipEq2 = 1; kf->SkipEq3 = 1; kf->SkipEq4 = 1; kf->SkipEq5 = 1;
|
||
|
||
memcpy(kf->F_data , gEstimateKF_F , sizeof(gEstimateKF_F));
|
||
memcpy(kf->B_data , gEstimateKF_B , sizeof(gEstimateKF_B));
|
||
memcpy(kf->H_data , gEstimateKF_H , sizeof(gEstimateKF_H));
|
||
// set rest of memory to 0
|
||
DWT_GetDeltaT(&observer->DWT_CNT);
|
||
}
|
||
//由当前时刻的状态量预测dt时刻后的
|
||
void Observer_Estimate(BalanceObserver* observer,float* zk,float* uk,float L0)
|
||
{
|
||
float L2;//腿长的平方
|
||
arm_power_f32(&L0,1,&L2);
|
||
KalmanFilter_t *kf = &observer->stateEstimateKF;
|
||
|
||
float A[6],B[6];
|
||
for(int i = 0 ;i<6;i++)
|
||
{
|
||
A[i] = p_A[i][0] * L2 + p_A[i][1] * L0 + p_A[i][2];
|
||
B[i] = p_B[i][0] * L2 + p_B[i][1] * L0 + p_B[i][2];
|
||
}
|
||
//F矩阵更新
|
||
kf->F_data[6] = A[0] * dt; kf->F_data[10] = A[1] * dt;
|
||
kf->F_data[18] = A[2] * dt; kf->F_data[22] = A[3] * dt;
|
||
kf->F_data[30] = A[4] * dt; kf->F_data[34] = A[5] * dt;
|
||
//B矩阵更新
|
||
kf->B_data[2] = B[0]; kf->B_data[3] = B[1];
|
||
kf->B_data[6] = B[2]; kf->B_data[7] = B[3];
|
||
kf->B_data[10] = B[4]; kf->B_data[11] = B[5];
|
||
//测量值更新
|
||
memcpy(kf->MeasuredVector,zk,6*4 );
|
||
//测量值更新
|
||
memcpy(kf->ControlVector,uk,2*4);
|
||
|
||
Kalman_Filter_Update(kf);
|
||
|
||
memcpy(observer->Estimate_X,kf->xhatminus_data,6*4);
|
||
} |