132 lines
4.6 KiB
C
132 lines
4.6 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] = {
|
||
{-2089.29388440040,913.777119858069,161.725026756580},
|
||
{-48.4503739138708,21.1903377794882,3.75037618024515},
|
||
{349.621231737198,-308.272343710195,14.5058784104114},
|
||
{8.10765758344078,-7.14878382193259,1.73804857662225},
|
||
{-126.242793963670,111.312353023678,-5.23784728498450},
|
||
{-2.92754916727612,2.58131475207839,21.1973941504635}
|
||
};
|
||
|
||
static const float p_B[6][3] = {
|
||
{-5.36398695946053,59.5647709213161,-40.3119963098152},
|
||
{127.834870314177,-113.128844359679,30.8319480335393},
|
||
{-33.9678204232552,24.3044065007359,0.798747072246912},
|
||
{13.4736135478509,-6.23400169559697,-0.818937686872829},
|
||
{12.2652521237935,-8.77594351760660,1.29067140754752},
|
||
{-4.86511248363210,2.25100114119497,2.36237087051996},
|
||
};
|
||
|
||
|
||
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为常量
|
||
|
||
//后验估计协方差初始值
|
||
static float P[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};
|
||
// R Q矩阵初始值（其实这里设置多少都无所谓）
|
||
static float Q[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};
|
||
static float R[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};
|
||
void Observer_Init(BalanceObserver* observer)
|
||
{
|
||
KalmanFilter_t *kf = &observer->stateEstimateKF;
|
||
Kalman_Filter_Init(kf,6,2,6);
|
||
|
||
for (uint8_t i = 0; i < 36; i += 7)
|
||
{
|
||
// 初始化过程噪声与量测噪声
|
||
Q[i] = 1.0f;//process_noise;
|
||
R[i] = 0.01f;//measure_noise;
|
||
}
|
||
|
||
//暂时只使用状态预测一个步骤
|
||
//kf->UseAutoAdjustment = 1;
|
||
//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));
|
||
memcpy(kf->Q_data , Q,sizeof(Q));
|
||
memcpy(kf->R_data , R,sizeof(R));
|
||
// 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);
|
||
|
||
for(uint8_t i = 0; i < 6; i++)
|
||
kf->MeasuredVector[i] = zk[i];
|
||
for(uint8_t i = 0; i < 2; i++)
|
||
kf->ControlVector[i] = uk[i];
|
||
|
||
Kalman_Filter_Update(kf);
|
||
|
||
// 提取估计值
|
||
for (uint8_t i = 0; i < 2; i++)
|
||
{
|
||
observer->Estimate_X[i] = kf->FilteredValue[i];
|
||
}
|
||
|
||
|
||
} |