添加速度估计，未进行速度融合

application/chassis/balance.c

@@ -17,6 +17,7 @@
 #include "bsp_dwt.h"
 #include "bsp_log.h"
 #include "lqr_calc.h"
+#include "speed_estimation.h"
 
 // 计时变量
 static uint32_t balance_dwt_cnt;
@@ -190,6 +191,8 @@ void BalanceTask()
     // 将五连杆映射成单杆
     Link2Leg(&l_side, &chassis);
     Link2Leg(&r_side, &chassis);
+    // 通过卡尔曼滤波估计机体速度
+    SpeedEstimation(&l_side, &r_side, &chassis, Chassis_IMU_data, del_t);
     // 根据单杆计算处的角度和杆长,计算反馈增益
     CalcLQR(&l_side, &chassis);
     CalcLQR(&r_side, &chassis);