wheel_legged/application/chassis/observer.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);
}