diff --git a/CMakeLists.txt b/CMakeLists.txt
index d48aef4..2255c2f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -35,6 +35,7 @@ add_compile_options(-ffunction-sections -fdata-sections -fno-common -fmessage-le
 # Enable assembler files preprocessing
 add_compile_options($<$<COMPILE_LANGUAGE:ASM>:-x$<SEMICOLON>assembler-with-cpp>)
 
+
 if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release")
     message(STATUS "Maximum optimization for speed")
     add_compile_options(-Ofast)
diff --git a/application/balance/estimator.c b/application/balance/estimator.c
new file mode 100644
index 0000000..9491b82
--- /dev/null
+++ b/application/balance/estimator.c
@@ -0,0 +1,56 @@
+//
+// Created by SJQ on 2024/3/7.
+//
+
+#include "estimator.h"
+static const float dt = 1e-3f;
+//状态转移矩阵
+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.1f,
+                     0.1f, 100};
+// R  Q矩阵初始值（其实这里设置多少都无所谓）
+static float Q[4] = {0.01f, 0,
+                     0, 0.01f};
+static float R[4] = {100000, 0,
+                     0, 100000,};
+void estimator_init(estimator_t *est ,float process_noise, float measure_noise) {
+    Kalman_Filter_Init(&est->EstimateKF_,2,1,2);
+    est->EstimateKF_.UseAutoAdjustment = 1;
+
+    for (uint8_t i = 0; i < 4; i += 3)
+    {
+        // 初始化过程噪声与量测噪声
+        Q[i] = process_noise;
+        R[i] = measure_noise;
+    }
+    memcpy(est->EstimateKF_.F_data,F,sizeof(F));
+    memcpy(est->EstimateKF_.B_data,B,sizeof(B));
+    memcpy(est->EstimateKF_.H_data,H,sizeof(H));
+    memcpy(est->EstimateKF_.Q_data,Q,sizeof(Q));
+    memcpy(est->EstimateKF_.R_data,R,sizeof(R));
+
+    DWT_GetDeltaT(&est->DWT_CNT_);
+}
+
+void estimator_update(estimator_t *est ,float x,float x_dot,float ax)  {
+    est->EstimateKF_.MeasuredVector[0] = x;
+    est->EstimateKF_.MeasuredVector[1] = x_dot;
+
+    est->EstimateKF_.ControlVector[0] = ax;
+
+    Kalman_Filter_Update(&est->EstimateKF_);
+
+    // 提取估计值
+    for (uint8_t i = 0; i < 2; i++)
+    {
+        est->Estimate_X_[i] = est->EstimateKF_.FilteredValue[i];
+    }
+}
diff --git a/application/balance/estimator.cpp b/application/balance/estimator.cpp
deleted file mode 100644
index 4268870..0000000
--- a/application/balance/estimator.cpp
+++ /dev/null
@@ -1,62 +0,0 @@
-//
-// 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];
-}
\ No newline at end of file
diff --git a/application/balance/estimator.h b/application/balance/estimator.h
index bbd81dc..4fce430 100644
--- a/application/balance/estimator.h
+++ b/application/balance/estimator.h
@@ -6,19 +6,15 @@
 #ifndef WHEEL_LEGGED_ESTIMATOR_H
 #define WHEEL_LEGGED_ESTIMATOR_H
 
-
-class estimator {
-public:
-    estimator(float process_noise, float measure_noise);
-    void get_result(float state[2]);
-    void update(float x,float x_dot,float ax);
-
-private:
+typedef struct
+{
     KalmanFilter_t EstimateKF_; //使用KF作为状态观测器
     float Estimate_X_[2]; //观测器估计的状态量
     uint32_t DWT_CNT_;  //计时用
     float dt_;
-};
+}estimator_t;
 
+void estimator_init(estimator_t *est ,float process_noise, float measure_noise);
+void estimator_update(estimator_t *est ,float x,float x_dot,float ax);
 
 #endif //WHEEL_LEGGED_ESTIMATOR_H
diff --git a/application/chassis/balance_old.c b/application/chassis/balance_old.c
index cd9b5f6..f3ac689 100644
--- a/application/chassis/balance_old.c
+++ b/application/chassis/balance_old.c
@@ -4,10 +4,12 @@
 
 #include "balance_old.h"
 
-static void mylqr_k(float L0, float K[12]);
+static void mylqr_k(float L0, float K_mat[12]);
 
 void Balance_Control_Init(Balance_Control_t* balanceControl,Balance_Init_config_s InitConfig)
 {
+
+    balanceControl->target_L0 = 0.15f;
     Leg_Init(&balanceControl->leg_right,&InitConfig.legInitConfig);
     Leg_Init(&balanceControl->leg_left,&InitConfig.legInitConfig);
 
@@ -15,6 +17,9 @@ void Balance_Control_Init(Balance_Control_t* balanceControl,Balance_Init_config_
     PIDInit(&balanceControl->leg_coordination_PID,&InitConfig.leg_cor_PID_config);
 
     PIDInit(&balanceControl->leg_length_PID,&InitConfig.legInitConfig.length_PID_conf);
+
+    estimator_init(&balanceControl->v_estimator,100,1);
+    Observer_Init(&balanceControl->state_observer);
     memset(balanceControl->state,0,6);
     memset(balanceControl->target_state,0,6);
 }
@@ -23,28 +28,47 @@ void Balance_Control_Init(Balance_Control_t* balanceControl,Balance_Init_config_
 float calculate_F(float MotionAccel_z,float theta,float theta_dot,float L,float L_dot,float F_fb,float Tp_fb);
 
 // leg_phi[4] = {phi1_r,phi4_r,phi1_l,phi4_l}
-void Balance_feedback_update(Balance_Control_t* balanceControl,float leg_phi[4],float leg_phi_dot[4],float body_phi,float body_phi_dot,float x,float x_dot,float MotionAccel_z)
+void Balance_feedback_update(Balance_Control_t* balanceControl,float leg_phi[4],float leg_phi_dot[4],float body_phi,float body_phi_dot,float x,float x_dot,const float MotionAccel[3])
 {
-    Leg_State_update(&balanceControl->leg_right,leg_phi[0],leg_phi[1],leg_phi_dot[0],leg_phi_dot[1]);
-    Leg_State_update(&balanceControl->leg_left,leg_phi[2],leg_phi[3],leg_phi_dot[2],leg_phi_dot[3]);
+    LegInstance *leg_r = &balanceControl->leg_right;
+    LegInstance *leg_l = &balanceControl->leg_left;
+
+    Leg_State_update(leg_r,leg_phi[0],leg_phi[1],leg_phi_dot[0],leg_phi_dot[1]);
+    Leg_State_update(leg_l,leg_phi[2],leg_phi[3],leg_phi_dot[2],leg_phi_dot[3]);
 
     float K_matrix[12]={0};
-    float L_average = (balanceControl->leg_right.legState.L0 + balanceControl->leg_left.legState.L0)/2;
-        //(balanceControl->leg_right.legState.L0 + balanceControl->leg_left.legState.L0)/2;
-    float alpha = PI/2 - balanceControl->leg_right.legState.phi0;
-
+    float L_average = leg_r->legState.L0; //(leg_r->legState.L0 + leg_l->legState.L0)/2;
+        //(leg_r->legState.L0 + leg_l->legState.L0)/2;
+    float alpha = PI/2 - leg_r->legState.phi0;
     float theta = alpha - body_phi;
-
 //    static uint32_t dot_cnt = 0;
 //    float dot_dt = DWT_GetDeltaT(&dot_cnt);
 //    float theta_dot1 = (balanceControl->state[0] - theta) / dot_dt;
+    float theta_dot = - leg_r->legState.phi0_dot - body_phi_dot;
 
-    float theta_dot = - balanceControl->leg_right.legState.phi0_dot - body_phi_dot;
+    //计算机体速度
+    float xb = x + 2 * leg_r->legState.L0 * arm_sin_f32(theta);
+    float xb_dot = x_dot + 2*(leg_r->legState.L0_dot* arm_sin_f32(theta) + leg_r->legState.L0 * theta_dot * arm_cos_f32(theta));
+
+    estimator_update(&balanceControl->v_estimator,xb,xb_dot,MotionAccel[0]);
 
     balanceControl->state[0] = theta;
-    balanceControl->state[1] = theta_dot;
-    balanceControl->state[2] = x; balanceControl->state[3] = x_dot;
-    balanceControl->state[4] = body_phi; balanceControl->state[5] = body_phi_dot;
+    balanceControl->state[1] = (1.0f - 0.75f) * balanceControl->state[1] +
+                               0.75f * theta_dot;
+    //balanceControl->state[1] = theta_dot;
+    balanceControl->state[2] = x;
+    balanceControl->state[3] = x_dot;
+
+    balanceControl->state[4] = body_phi;
+    balanceControl->state[5] = (1.0f - 0.75f) * balanceControl->state[5] +
+                                0.75f * body_phi_dot;
+//    balanceControl->state[5] = body_phi_dot;
+
+    //Observer_Estimate(&balanceControl->state_observer,balanceControl->state,balanceControl->balance_LQR.control_vector,L_average);
+
+    //地面支持力解算
+    //balanceControl->FN_right = calculate_F(MotionAccel[2],theta,theta_dot,leg_r->legState.L0,leg_r->legState.L0_dot,leg_r->legFeedback.F,leg_r->legFeedback.Tp);
+    //balanceControl->FN_left = calculate_F(MotionAccel[2],theta,theta_dot,leg_l->legState.L0,leg_l->legState.L0_dot,leg_l->legFeedback.F,leg_l->legFeedback.Tp);
 
     mylqr_k(L_average,K_matrix);
     LQR_set_K(&balanceControl->balance_LQR,K_matrix);
@@ -68,7 +92,7 @@ void Balance_Control_Loop(Balance_Control_t* balanceControl)
     float F_pid_out_l =  PIDCalculate(&leg_l->Length_PID,leg_l->legState.L0,leg_l->target_L0);
     //控制平均腿长
     float average_length = (leg_r->legState.L0 + leg_l->legState.L0)/2;
-    float F_pid_out=  PIDCalculate(&balanceControl->leg_length_PID,average_length,leg_r->target_L0);
+    float F_pid_out = PIDCalculate(&balanceControl->leg_length_PID,average_length,balanceControl->target_L0);
     //float F_pid_out_l =  PIDCalculate(&balanceControl->leg_length_PID,average_length,leg_r->target_L0);
 
     float Tp_pid_out_r = PIDCalculate(&leg_r->Phi0_PID,leg_r->legState.phi0,leg_r->target_phi0);
@@ -78,8 +102,8 @@ void Balance_Control_Loop(Balance_Control_t* balanceControl)
 
     float cor_Tp = PIDCalculate(&balanceControl->leg_coordination_PID,coordination_err,0);
 
-    Leg_Input_update(&leg_r->legInput,F_pid_out + leg_r->F_feedforward + 20,Tp + cor_Tp);
-    Leg_Input_update(&leg_l->legInput,F_pid_out + leg_l->F_feedforward + 20,Tp - cor_Tp);
+    Leg_Input_update(&leg_r->legInput,F_pid_out + leg_r->F_feedforward + 30,Tp + cor_Tp);
+    Leg_Input_update(&leg_l->legInput,F_pid_out + leg_l->F_feedforward + 30,Tp - cor_Tp);
 
 //    Leg_Input_update(&leg_r->legInput,F_pid_out_r + 20 ,Tp + cor_Tp);
 //    Leg_Input_update(&leg_l->legInput,F_pid_out_l + 20 ,Tp - cor_Tp);
@@ -88,9 +112,22 @@ void Balance_Control_Loop(Balance_Control_t* balanceControl)
     VMC_getT(leg_l);
 }
 
-void target_x_set(Balance_Control_t* balanceControl,float x)
+void target_x_set(Balance_Control_t* balanceControl,float add_x)
 {
-    balanceControl->target_state [2] = x;
+    balanceControl->target_state [2] += add_x;
+}
+
+void state_clear(Balance_Control_t* balanceControl)
+{
+    //清除位移状态量
+    balanceControl->target_state [2] = 0;
+    balanceControl->state[2] = 0;
+}
+
+void target_L_set(Balance_Control_t* balanceControl,float add_L)
+{
+    balanceControl->target_L0 += add_L;
+    float_constrain(balanceControl->target_L0,0.15f,0.30f);
 }
 
 
@@ -103,47 +140,47 @@ void target_x_set(Balance_Control_t* balanceControl,float x)
  *                float K[12]
  * Return Type  : void
  */
-void mylqr_k(float L0, float K[12])
+void mylqr_k(float L0, float K_mat[12])
 {
     float t2;
     float t3;
 
-    float K_temp[12];
+    float K[12];
     /*     This function was generated by the Symbolic Math Toolbox version 9.2.
      */
     t2 = L0 * L0;
     t3 = t2 * L0;
-    /*     06-Mar-2024 21:40:29 */
-    K_temp[0] =
-            ((L0 * -161.752151F + t2 * 164.793869F) - t3 * 97.0541687F) - 1.77457297F;
-    K_temp[1] =
-            ((L0 * 96.1845703F - t2 * 284.733704F) + t3 * 279.889618F) + 3.29777265F;
-    K_temp[2] =
-            ((L0 * -15.8959455F - t2 * 17.009819F) + t3 * 16.7305775F) - 0.452344805F;
-    K_temp[3] =
-            ((L0 * 5.66845322F - t2 * 16.5131245F) + t3 * 14.656208F) + 0.677895188F;
-    K_temp[4] = ((L0 * -7.06628323F + t2 * 13.4040155F) - t3 * 8.83661747F) - 8.61461F;
-    K_temp[5] =
-            ((L0 * -28.798315F + t2 * 17.1446743F) + t3 * 15.3877935F) + 11.4884624F;
-    K_temp[6] =
-            ((L0 * -5.75727034F - t2 * 3.09837651F) + t3 * 10.1307449F) - 9.7942028F;
-    K_temp[7] =
-            ((L0 * -33.38451F + t2 * 35.8943558F) - t3 * 7.07260895F) + 11.7050676F;
-    K_temp[8] =
-            ((L0 * -83.7100143F + t2 * 109.333092F) - t3 * 54.9969864F) + 36.5699463F;
-    K_temp[9] =
-            ((L0 * 124.852882F - t2 * 253.387085F) + t3 * 193.295883F) + 55.7350769F;
-    K_temp[10] = ((L0 * -6.44499636F + t2 * 5.48124027F) - t3 * 0.516800761F) +
-            5.52106953F;
-    K_temp[11] =
-            ((L0 * 12.3147469F - t2 * 13.2560177F) + t3 * 1.4054811F) + 2.85080624F;
-
+    /*     11-Mar-2024 19:12:17 */
+    K[0] =
+            ((L0 * -173.845428F + t2 * 178.098602F) - t3 * 104.773026F) - 8.81736755F;
+    K[1] =
+            ((L0 * 150.104736F - t2 * 511.085876F) + t3 * 523.35144F) + 10.6000547F;
+    K[2] =
+            ((L0 * -16.623167F - t2 * 12.9591064F) + t3 * 11.4631195F) - 2.62765169F;
+    K[3] =
+            ((L0 * -1.31775725F - t2 * 24.5691776F) + t3 * 32.5003357F) + 5.97731924F;
+    K[4] =
+            ((L0 * 4.03619576F - t2 * 33.4860535F) + t3 * 47.0306854F) - 11.8519392F;
+    K[5] =
+            ((L0 * 23.7967739F - t2 * 163.993057F) + t3 * 207.196823F) + 8.34590435F;
+    K[6] =
+            ((L0 * 29.8367863F - t2 * 116.608421F) + t3 * 131.16246F) - 16.6295223F;
+    K[7] =
+            ((L0 * -16.2756023F - t2 * 56.7522507F) + t3 * 104.046684F) + 14.3186169F;
+    K[8] =
+            ((L0 * -104.948708F + t2 * 87.532341F) - t3 * 7.19816732F) + 51.9084816F;
+    K[9] =
+            ((L0 * 172.631012F - t2 * 274.727661F) + t3 * 150.935028F) + 42.625248F;
+    K[10] =
+            ((L0 * -11.3688688F + t2 * 19.2530308F) - t3 * 15.6853495F) + 6.75911F;
+    K[11] =
+            ((L0 * 10.4498978F - t2 * 4.97098875F) - t3 * 7.18331099F) + 2.7609446F;
     //matlab赋值顺序不同
     for (int i = 0; i < 6; ++i) {
-        K[i] = K_temp[2*i];
+        K_mat[i] = K[2*i];
     }
     for (int i = 0; i < 6; ++i) {
-        K[6+i] = K_temp[2*i+1];
+        K_mat[6+i] = K[2*i+1];
     }
 }
 
diff --git a/application/chassis/balance_old.h b/application/chassis/balance_old.h
index 28e39c4..4128c73 100644
--- a/application/chassis/balance_old.h
+++ b/application/chassis/balance_old.h
@@ -9,6 +9,8 @@
 #include "LQR_old.h"
 #include "controller.h"
 #include "kalman_filter.h"
+#include "estimator.h"
+#include "observer.h"
 typedef struct
 {
     LegInstance leg_right;
@@ -19,9 +21,13 @@ typedef struct
 
     float state[6];
     float target_state[6];
+    float target_L0; //平均腿长目标值
+    float FN_left; //地面支持力
+    float FN_right;
 
     LQRInstance balance_LQR;
-
+    estimator_t v_estimator;
+    BalanceObserver state_observer;
 
 }Balance_Control_t;
 
@@ -36,8 +42,10 @@ typedef struct
 
 void Balance_Control_Init(Balance_Control_t* balanceControl,Balance_Init_config_s InitConfig);
 //void Balance_feedback_update(Balance_Control_t* balanceControl,float leg_phi[4],float body_phi,float body_phi_dot,float x,float x_dot);
-void Balance_feedback_update(Balance_Control_t* balanceControl,float leg_phi[4],float leg_phi_dot[4],float body_phi,float body_phi_dot,float x,float x_dot,float MotionAccel_z);
+void Balance_feedback_update(Balance_Control_t* balanceControl,float leg_phi[4],float leg_phi_dot[4],float body_phi,float body_phi_dot,float x,float x_dot,const float MotionAccel[3]);
 void Balance_Control_Loop(Balance_Control_t* balanceControl);
-void target_x_set(Balance_Control_t* balanceControl,float x);
+void target_x_set(Balance_Control_t* balanceControl,float add_x);
+void target_L_set(Balance_Control_t* balanceControl,float add_L);
+void state_clear(Balance_Control_t* balanceControl);
 
 #endif //WHEEL_LEGGED_BALANCE_OLD_H
diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c
index 5cddf98..b72d98c 100644
--- a/application/chassis/chassis.c
+++ b/application/chassis/chassis.c
@@ -24,6 +24,7 @@
 
 #include "balance_old.h"
 #include "observer.h"
+#include "estimator.h"
 
 #include "general_def.h"
 #include "bsp_dwt.h"
@@ -91,11 +92,21 @@ void ChassisInit()
 
     Motor_Init_Config_s wheel_motor_config = {
             .can_init_config.can_handle = &hcan1,
+            .controller_param_init_config ={
+                    .speed_PID = {
+                            .Kp = -100,  // 50
+                            .Ki = 0, // 200
+                            .Kd = 0,
+                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
+                            .IntegralLimit = 3000,
+                            .MaxOut = 20000,
+                    },
+            },
             .controller_setting_init_config = {
                     .angle_feedback_source = MOTOR_FEED,
                     .speed_feedback_source = MOTOR_FEED,
                     .outer_loop_type = OPEN_LOOP,
-                    .close_loop_type = OPEN_LOOP,//| CURRENT_LOOP,
+                    .close_loop_type = OPEN_LOOP,
             },
             .motor_type = LK9025,
     };
@@ -172,9 +183,9 @@ void ChassisInit()
                     .F_feedforward = 0,
             },
             .leg_cor_PID_config = {
-                    .Kp = 25,
+                    .Kp = 25.0f,//25,//25,
                     .Ki = 0,
-                    .Kd = 3,
+                    .Kd = 3.0f,//3,
                     .MaxOut = 3,
                     .Improve =  PID_Derivative_On_Measurement| PID_DerivativeFilter,
                     .Derivative_LPF_RC = 0.01f,
@@ -189,7 +200,7 @@ void ChassisInit()
     Chassis_IMU_data = INS_Init(); // 底盘IMU初始化
     //转向环
     PID_Init_Config_s turn_pid_cfg = {
-            .Kp = 10.0f,//1,
+            .Kp = 10.0f,//10.0f,//1,
             .Ki = 0,
             .Kd = 0.0f,
             .MaxOut = 2,
@@ -261,8 +272,6 @@ static void Chassis_State_update()
     float body_phi = Chassis_IMU_data->Pitch * DEGREE_2_RAD;
     float body_phi_dot = Chassis_IMU_data->Gyro[X];
 
-    float total_angle = (-wheel_motor_r->measure.total_angle + wheel_motor_l->measure.total_angle)/2;
-
     //陀螺仪积分获取位移和速度
     static uint32_t integral_cnt = 0;
     static float imu_v,imu_x;
@@ -277,18 +286,32 @@ static void Chassis_State_update()
 
 
     //float theta_dot1 = (balanceControl->state[0] - theta) / dot_dt;
+    static float left_offset = 0;
+    static float right_offset = 0;
+    //倒地需要清零总位移数据
+    if(chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
+    {
+        left_offset = wheel_motor_l->measure.total_angle;
+        right_offset = wheel_motor_r->measure.total_angle;
+    }
 
+    float x_l = wheel_motor_l->measure.total_angle - left_offset;
+    float x_r = wheel_motor_r->measure.total_angle - right_offset;
+    //驱动轮位移与速度
+//    float total_angle = (- wheel_motor_r->measure.total_angle + wheel_motor_l->measure.total_angle)/2;
+//    float x =  (total_angle * DEGREE_2_RAD ) * RADIUS_WHEEL;
+//    float speed = (- wheel_motor_r->measure.speed_rads + wheel_motor_l->measure.speed_rads)/2;
+//    float x_dot = speed * RADIUS_WHEEL;
 
-    float x =  (total_angle * DEGREE_2_RAD ) * 0.135f/2;
-
+    float total_angle = (- x_r + x_l)/2;
+    float x =  (total_angle * DEGREE_2_RAD ) * RADIUS_WHEEL;
     float speed = (- wheel_motor_r->measure.speed_rads + wheel_motor_l->measure.speed_rads)/2;
+    float x_dot = speed * RADIUS_WHEEL;
 
-    float x_dot = speed * 0.135f/2;
+//    Leg_feedback_update(&BalanceControl.leg_right,motor_rf->measure.real_current / HT_CMD_COEF,motor_rb->measure.real_current / HT_CMD_COEF);
+//    Leg_feedback_update(&BalanceControl.leg_left,motor_lf->measure.real_current / HT_CMD_COEF,motor_lb->measure.real_current / HT_CMD_COEF);
 
-    Balance_feedback_update(&BalanceControl,leg_phi,leg_phi_dot,body_phi,body_phi_dot,x,x_dot,Chassis_IMU_data->MotionAccel_n[2]);
-
-    Leg_feedback_update(&BalanceControl.leg_right,motor_rf->measure.real_current / HT_CMD_COEF,motor_rb->measure.real_current / HT_CMD_COEF);
-    Leg_feedback_update(&BalanceControl.leg_left,motor_lf->measure.real_current / HT_CMD_COEF,motor_lb->measure.real_current / HT_CMD_COEF);
+    Balance_feedback_update(&BalanceControl,leg_phi,leg_phi_dot,body_phi,body_phi_dot,x,x_dot,Chassis_IMU_data->MotionAccel_n);
 
     if( ChassisState == FAIL )
     {
@@ -310,7 +333,7 @@ void ChassisTask()
     chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm);
 #endif // CHASSIS_BOARD
 
-    if (chassis_cmd_recv.chassis_mode == CHASSIS_NO_FOLLOW)
+    if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
     { // 如果出现重要模块离线或遥控器设置为急停,让电机停止
         HTMotorStop(motor_lf);
         HTMotorStop(motor_rf);
@@ -318,6 +341,14 @@ void ChassisTask()
         HTMotorStop(motor_rb);
         LKMotorStop(wheel_motor_r);
         LKMotorStop(wheel_motor_l);
+
+        //清除相关pid控制量
+        PIDClear(&BalanceControl.leg_length_PID);
+        PIDClear(&Turn_Loop_PID);
+        PIDClear(&Roll_Loop_PID);
+        PIDClear(&BalanceControl.leg_coordination_PID);
+        //清除状态量
+        state_clear(&BalanceControl);
     }
     else
     { // 正常工作
@@ -339,10 +370,11 @@ void ChassisTask()
         break;
     case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
         //chassis_cmd_recv.wz = 1.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
+        chassis_cmd_recv.wz = 0.01f * chassis_cmd_recv.offset_angle* abs(chassis_cmd_recv.offset_angle);
         break;
     case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
         //chassis_cmd_recv.wz = 4000;
-        chassis_cmd_recv.wz = 1.0f * chassis_cmd_recv.offset_angle* abs(chassis_cmd_recv.offset_angle);
+        chassis_cmd_recv.wz = 2.0f;
         break;
     default:
         break;
@@ -367,74 +399,61 @@ void ChassisTask()
     //获取数据并计算状态量
     Chassis_State_update();
 
-    float vofa_send_data[8];
+    float vofa_send_data[12];
 
-    BalanceControl.leg_right.target_L0 += chassis_cmd_recv.vy/5280 * 0.0003f;
-    BalanceControl.leg_right.target_L0 = float_constrain(BalanceControl.leg_right.target_L0,0.15f,0.30f);
-    BalanceControl.leg_right.target_phi0 = PI/2;
-
-    BalanceControl.leg_left.target_L0 = BalanceControl.leg_right.target_L0;
-    BalanceControl.leg_left.target_L0 = float_constrain(BalanceControl.leg_left.target_L0,0.15f,0.30f);
-    BalanceControl.leg_left.target_phi0 = PI/2;
-
-
-    vofa_send_data[0] = BalanceControl.leg_right.legState.L0;
-    vofa_send_data[1] = BalanceControl.leg_left.legState.L0;
-    vofa_send_data[2] = BalanceControl.leg_length_PID.Ref;
-    vofa_send_data[3] = BalanceControl.leg_length_PID.Measure;
-    vofa_send_data[4] = BalanceControl.leg_right.legInput.F;
-    vofa_send_data[5] = BalanceControl.leg_left.legInput.F;
-    vofa_send_data[6] = Observer.Estimate_X[3];
-    vofa_send_data[7] = BalanceControl.state[3];
-
-
-
-    vofa_justfloat_output(vofa_send_data,32,&huart1);
+//    for(int i = 0; i<6;i++)
+//        vofa_send_data[i] = BalanceControl.state[i];
+//    for(int i = 0; i<6;i++)
+//        vofa_send_data[i+6] = BalanceControl.state_observer.Estimate_X[i];
+    vofa_justfloat_output(vofa_send_data,12*4,&huart1);
 
     static float target_x = 0;
     static float target_yaw = 0;
-    if(chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE)  // 右侧开关状态[下],底盘跟随云台
+    if(chassis_cmd_recv.chassis_mode != CHASSIS_ZERO_FORCE)  // 右侧开关状态[下],底盘跟随云台
     {
         float Roll_pid_out = PIDCalculate(&Roll_Loop_PID,-Chassis_IMU_data->Roll,0);
         //将roll轴自平衡PID输出直接叠加到前馈支持力上
         BalanceControl.leg_right.F_feedforward = Roll_pid_out;
         BalanceControl.leg_left.F_feedforward = -Roll_pid_out;
 
-        target_x += chassis_cmd_recv.vx/5280 * 0.003f;
-        target_x_set(&BalanceControl,target_x);
-
+        //target_x += chassis_cmd_recv.vx/5280 * 0.003f;
+        target_x_set(&BalanceControl,chassis_cmd_recv.vx/5280 * 0.003f);
+        target_L_set(&BalanceControl,chassis_cmd_recv.vy/5280 * 0.0003f);
         Balance_Control_Loop(&BalanceControl);
 
+        static float wz_feedback;
+        wz_feedback = (1.0f - 0.75f) * wz_feedback + 0.75f * Chassis_IMU_data->Gyro[Z];
+        float turn_T = PIDCalculate(&Turn_Loop_PID,Chassis_IMU_data->Gyro[Z],chassis_cmd_recv.wz);
 
+        static float T,wheel_current_r,wheel_current_l;
 
-        target_yaw = 0.0f;//chassis_cmd_recv.wz * 0.001f;//Chassis_IMU_data->Gyro[Z]
+        T = BalanceControl.balance_LQR.control_vector[0];//float_constrain(BalanceControl.balance_LQR.control_vector[0],-4,4);
 
-        //float turn_T = -PIDCalculate(&Turn_Loop_PID,chassis_cmd_recv.offset_angle,0);
-        //float turn_T = -1.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
-        float turn_T = PIDCalculate(&Turn_Loop_PID,Chassis_IMU_data->Gyro[Z],target_yaw);
+        //T = (1.0f - 0.75f) * T + 0.75f * BalanceControl.balance_LQR.control_vector[0];//float_constrain(BalanceControl.balance_LQR.control_vector[0],-4,4);
 
-        float T = float_constrain(BalanceControl.balance_LQR.control_vector[0],-4,4);
+//        wheel_current_r = (1.0f - 0.75f) * wheel_current_r + 0.75f * (T+turn_T) * LK_TORQUE_2_CMD;//+turn_T
+//        wheel_current_l = (1.0f - 0.75f) * wheel_current_l + 0.75f * (T-turn_T) * LK_TORQUE_2_CMD;//-turn_T
 
-        float wheel_current_r = (T+turn_T) * LK_TORQUE_2_CMD;
-        float wheel_current_l = (T-turn_T) * LK_TORQUE_2_CMD;
+        wheel_current_r = (T+turn_T) * LK_TORQUE_2_CMD;
+        wheel_current_l = (T-turn_T) * LK_TORQUE_2_CMD;
 
-        float_constrain(wheel_current_r,-2000,2000);
-        float_constrain(wheel_current_l,-2000,2000);
+        float_constrain(wheel_current_r,-4000,4000);
+        float_constrain(wheel_current_l,-4000,4000);
 
-        if (ChassisState == FAIL) // 倒地 未起身
-        {
-            HTMotorSetRef(motor_rf,0);
-            HTMotorSetRef(motor_rb,0);
-
-            HTMotorSetRef(motor_lf,0);
-            HTMotorSetRef(motor_lb,0);
-
-            PIDClear(&BalanceControl.leg_length_PID);
-            PIDClear(&Turn_Loop_PID);
-            PIDClear(&Roll_Loop_PID);
-            PIDClear(&BalanceControl.leg_coordination_PID);
-        }
-        else
+//        if (ChassisState == FAIL) // 倒地 未起身
+//        {
+//            HTMotorSetRef(motor_rf,0);
+//            HTMotorSetRef(motor_rb,0);
+//
+//            HTMotorSetRef(motor_lf,0);
+//            HTMotorSetRef(motor_lb,0);
+//
+//            PIDClear(&BalanceControl.leg_length_PID);
+//            PIDClear(&Turn_Loop_PID);
+//            PIDClear(&Roll_Loop_PID);
+//            PIDClear(&BalanceControl.leg_coordination_PID);
+//        }
+//        else
         {
             HTMotorSetRef(motor_rf,-BalanceControl.leg_right.legOutput.T1 * HT_CMD_COEF);
             HTMotorSetRef(motor_rb,-BalanceControl.leg_right.legOutput.T2 * HT_CMD_COEF);
@@ -450,12 +469,12 @@ void ChassisTask()
 
         LKMotorSetRef(wheel_motor_r,wheel_current_r);
         LKMotorSetRef(wheel_motor_l,wheel_current_l);
-//        LKMotorSetRef(wheel_motor_r,100);
-//        LKMotorSetRef(wheel_motor_l,100);
+//        LKMotorSetRef(wheel_motor_r,0);
+//        LKMotorSetRef(wheel_motor_l,0);
 
-        Observer_Estimate(&Observer,BalanceControl.state,BalanceControl.balance_LQR.control_vector, BalanceControl.leg_right.legState.L0);
-        Leg_feedback_update(&BalanceControl.leg_right,motor_rf->measure.real_current/HT_CMD_COEF,motor_rb->measure.real_current/HT_CMD_COEF);
-        Leg_feedback_update(&BalanceControl.leg_left,motor_lf->measure.real_current/HT_CMD_COEF,motor_lb->measure.real_current/HT_CMD_COEF);
+//        Observer_Estimate(&Observer,BalanceControl.state,BalanceControl.balance_LQR.control_vector, BalanceControl.leg_right.legState.L0);
+//        Leg_feedback_update(&BalanceControl.leg_right,motor_rf->measure.real_current/HT_CMD_COEF,motor_rb->measure.real_current/HT_CMD_COEF);
+//        Leg_feedback_update(&BalanceControl.leg_left,motor_lf->measure.real_current/HT_CMD_COEF,motor_lb->measure.real_current/HT_CMD_COEF);
     }
     else{
         PIDClear(&BalanceControl.leg_length_PID);
diff --git a/application/chassis/leg_old.c b/application/chassis/leg_old.c
index e76af67..1c398cc 100644
--- a/application/chassis/leg_old.c
+++ b/application/chassis/leg_old.c
@@ -54,12 +54,14 @@ void Leg_State_update(LegInstance* legInstance, float phi1,float phi4 ,float phi
 
 
     //对phi0求导
-//    leg_state->dt = DWT_GetDeltaT(&leg_state->DWT_CNT);
-//    if(leg_state->last_phi0>=0)
-//        leg_state->phi0_dot = (leg_state->phi0-leg_state->last_phi0)/leg_state->dt;
-//    else
-//        leg_state->phi0_dot = 0;
-//    leg_state->last_phi0 = leg_state->phi0;
+    leg_state->dt = DWT_GetDeltaT(&leg_state->DWT_CNT);
+    //if(leg_state->last_phi0>=0)
+        leg_state->phi0_dot_num = (leg_state->phi0-leg_state->last_phi0)/leg_state->dt;
+    //else
+        //leg_state->phi0_dot_num = 0;
+    leg_state->last_phi0 = leg_state->phi0;
+
+
     leg_state->phi0_dot = get_dphi0(phi1,phi4,phi1_dot,phi4_dot);
     leg_state->L0_dot = get_dL0(phi1,phi4,phi1_dot,phi4_dot);
 }
diff --git a/application/chassis/leg_old.h b/application/chassis/leg_old.h
index 54d97ac..efbe88b 100644
--- a/application/chassis/leg_old.h
+++ b/application/chassis/leg_old.h
@@ -21,8 +21,10 @@ typedef struct
     float dt;        //用于计算时间并求导
     float last_phi0;
     float phi0_dot;
+    float phi0_dot_num;
 
     float L0_dot;
+    float L0_dot_num;
 
 }Leg_State_t;
 
diff --git a/application/chassis/observer.c b/application/chassis/observer.c
index 20a8423..3710da6 100644
--- a/application/chassis/observer.c
+++ b/application/chassis/observer.c
@@ -7,22 +7,23 @@
 #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}
+        {-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] = {
-        {-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}
+        {-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},
 };
 
 
@@ -44,19 +45,47 @@ const float gEstimateKF_H[36] = {1, 0, 0, 0, 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 = 0;
-    kf->SkipEq2 = 1; kf->SkipEq3 = 1; kf->SkipEq4 = 1; kf->SkipEq5 = 1;
+    //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);
 }
@@ -82,11 +111,22 @@ void Observer_Estimate(BalanceObserver* observer,float* zk,float* uk,float L0)
     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);
+//    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);
 
-    memcpy(observer->Estimate_X,kf->xhatminus_data,6*4);
+    // 提取估计值
+    for (uint8_t i = 0; i < 2; i++)
+    {
+        observer->Estimate_X[i] = kf->FilteredValue[i];
+    }
+
+
 }
\ No newline at end of file
diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c
index 5300eb2..0ad8c45 100644
--- a/application/cmd/robot_cmd.c
+++ b/application/cmd/robot_cmd.c
@@ -73,7 +73,7 @@ void RobotCMDInit()
 #endif // GIMBAL_BOARD
     gimbal_cmd_send.pitch = 0;
 
-    robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入
+    robot_state = ROBOT_STOP; // 轮腿  上电后进入ROBOT_STOP 保证安全(对应倒地状态）
 }
 
 /**
diff --git a/application/robot_def.h b/application/robot_def.h
index 8fca5ec..087ba3b 100644
--- a/application/robot_def.h
+++ b/application/robot_def.h
@@ -17,8 +17,8 @@
 #include "stdint.h"
 
 /* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */
-#define ONE_BOARD // 单板控制整车
-//#define CHASSIS_BOARD //底盘板
+//#define ONE_BOARD // 单板控制整车
+#define CHASSIS_BOARD //底盘板
 // #define GIMBAL_BOARD  //云台板
 
 #define VISION_USE_VCP  // 使用虚拟串口发送视觉数据
@@ -40,7 +40,7 @@
 #define TRACK_WIDTH 0.3f             // 横向轮距(左右平移方向)
 #define CENTER_GIMBAL_OFFSET_X 0    // 云台旋转中心距底盘几何中心的距离,前后方向,云台位于正中心时默认设为0
 #define CENTER_GIMBAL_OFFSET_Y 0    // 云台旋转中心距底盘几何中心的距离,左右方向,云台位于正中心时默认设为0
-#define RADIUS_WHEEL 0.06f             // 轮子半径
+#define RADIUS_WHEEL 0.1f             // 轮子半径
 #define REDUCTION_RATIO_WHEEL 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换
 
 #define GYRO2GIMBAL_DIR_YAW 1   // 陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反
diff --git a/modules/motor/LKmotor/LK9025.c b/modules/motor/LKmotor/LK9025.c
index 75f9047..5556f3d 100644
--- a/modules/motor/LKmotor/LK9025.c
+++ b/modules/motor/LKmotor/LK9025.c
@@ -143,18 +143,18 @@ void LKMotorControl()
 
         if ((setting->close_loop_type & SPEED_LOOP) && setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))
         {
-            if (setting->angle_feedback_source == OTHER_FEED)
+            if (setting->speed_feedback_source == OTHER_FEED)
                 pid_measure = *motor->other_speed_feedback_ptr;
             else
                 pid_measure = measure->speed_rads;
-            pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
+            pid_ref = PIDCalculate(&motor->speed_PID, pid_measure, pid_ref);
             if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
                 pid_ref += *motor->current_feedforward_ptr;
         }
 
         if (setting->close_loop_type & CURRENT_LOOP)
         {
-            pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref);
+            //pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref);
         }
 
         set = (int16_t)pid_ref;