diff --git a/application/chassis/balance.c b/application/chassis/balance.c
index eb902c0..8e3ffb4 100644
--- a/application/chassis/balance.c
+++ b/application/chassis/balance.c
@@ -15,6 +15,8 @@
 #include "arm_math.h" // 需要用到较多三角函数
 #include "bsp_dwt.h"
 #include "bsp_log.h"
+#include "linkNleg.h"
+#include "speed_estimation.h"
 
 
 static uint32_t balance_dwt_cnt;
diff --git a/application/chassis/linkNleg.h b/application/chassis/linkNleg.h
new file mode 100644
index 0000000..f1ac1de
--- /dev/null
+++ b/application/chassis/linkNleg.h
@@ -0,0 +1,79 @@
+#include "arm_math.h"
+#include "balance.h"
+#include "math.h"
+#include "user_lib.h"
+
+/* 计算的T_hip和F_Leg映射为关节电机输出 */
+void VMCProject(LinkNPodParam *p)
+{
+    float phi12 = p->phi1 - p->phi2;
+    float phi34 = p->phi3 - p->phi4;
+    float phi32 = p->phi3 - p->phi2;
+    float phi53 = p->phi5 - p->phi3;
+    float phi52 = p->phi5 - p->phi2;
+    float F_m_L = p->F_leg * p->leg_len;
+    p->T_back = (THIGH_LEN * msin(phi12) * (F_m_L * msin(phi53) + p->T_hip * mcos(phi53))) / (p->leg_len * msin(phi32));
+    p->T_front = (CALF_LEN * msin(phi34) * (F_m_L * msin(phi52) + p->T_hip * mcos(phi52))) / (p->leg_len * msin(phi32));
+}
+
+/**
+ * @brief 根据关节角度和角速度,计算单杆长度和角度以及变化率
+ *
+ * @note 右侧视图
+ *  ___x
+ * |    1  _____  4
+ * |y     /     \
+ *      2 \     / 3
+ *         \   /
+ *          \./
+ *           5
+ * @param p 5连杆和腿的参数
+ */
+void Link2Leg(LinkNPodParam *p, ChassisParam *chassis)
+{
+    float xD, yD, xB, yB, BD, A0, B0, xC, yC;
+    p->coord[4] = xD = JOINT_DISTANCE + THIGH_LEN * mcos(p->phi4);
+    p->coord[5] = yD = THIGH_LEN * msin(p->phi4);
+    p->coord[0] = xB = THIGH_LEN * mcos(p->phi1);
+    p->coord[1] = yB = THIGH_LEN * msin(p->phi1);
+
+    BD = powf(xD - xB, 2) + powf(yD - yB, 2);
+    A0 = 2 * CALF_LEN * (xD - xB);
+    B0 = 2 * CALF_LEN * (yD - yB);
+    p->phi2 = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD);
+    p->coord[2] = xC = xB + CALF_LEN * mcos(p->phi2);
+    p->coord[3] = yC = yB + CALF_LEN * msin(p->phi2);
+    p->phi3 = atan2f(yC - yD, xC - xD);             // 稍后用于计算VMC
+
+    // theta and LegLength solve
+    p->phi5 = atan2f(yC, xC - JOINT_DISTANCE / 2);
+    p->leg_len = Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2));
+    p->theta = p->phi5 - 0.5 * PI - chassis->pitch; // 确定方向
+    p->height = p->leg_len * mcos(p->theta);
+
+    // 预测下一个时刻
+    static float predict_dt = 0.0001f;
+    float phi1_pred = p->phi1 + p->phi1_w * predict_dt; // 预测下一时刻的关节角度(利用关节角速度)
+    float phi4_pred = p->phi4 + p->phi4_w * predict_dt;
+
+    // 重新计算腿长和腿角度
+    xD = JOINT_DISTANCE + THIGH_LEN * mcos(phi4_pred);
+    yD = THIGH_LEN * msin(phi4_pred);
+    xB = 0 + THIGH_LEN * mcos(phi1_pred);
+    yB = THIGH_LEN * msin(phi1_pred);
+
+    BD = powf(xD - xB, 2) + powf(yD - yB, 2);
+    A0 = 2 * CALF_LEN * (xD - xB);
+    B0 = 2 * CALF_LEN * (yD - yB);
+    float phi2_pred = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD);
+    xC = xB + CALF_LEN * mcos(phi2_pred);
+    yC = yB + CALF_LEN * msin(phi2_pred);
+    float phi5_pred = atan2f(yC, xC - JOINT_DISTANCE / 2);
+
+    // 差分计算腿长变化率和腿角速度
+    p->phi2_w = (phi2_pred - p->phi2) / predict_dt; // 稍后用于修正轮速
+    p->phi5_w = (phi5_pred - p->phi5) / predict_dt;
+    p->legd = (Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2)) - p->leg_len) / predict_dt;
+    p->theta_w = ((phi5_pred - 0.5 * PI - (chassis->pitch + chassis->pitch_w) - p->theta) / predict_dt); // 可以不考虑机体? -predict_dt*chassis.pitch_w
+    p->height_v = p->legd * mcos(p->theta) - p->leg_len * msin(p->theta) * p->theta_w;
+}
diff --git a/application/chassis/speed_estimation.h b/application/chassis/speed_estimation.h
new file mode 100644
index 0000000..836c37d
--- /dev/null
+++ b/application/chassis/speed_estimation.h
@@ -0,0 +1,100 @@
+#include "balance.h"
+#include "user_lib.h"
+#include "ins_task.h"
+#include "general_def.h"
+
+#define EST_FINAL_LPF 0.005f // 最终速度的低通滤波系数
+static KalmanFilter_t kf;
+
+/**
+ * @brief 底盘为右手系
+ *
+ *    ^ y            左轮     右轮
+ *    |               |      |  前
+ *    |_____ >  x     |------|
+ *    z 轴从屏幕向外   |      |  后
+ */
+
+void SpeedEstInit()
+{   
+    // 使用kf同时估计速度和加速度
+    // Kalman_Filter_Init(&kf, 2, 0, 2);
+    // float F[4] = {1, 0.001, 0, 1};
+    // float Q[4] = {VEL_PROCESS_NOISE, 0, 0, ACC_PROCESS_NOISE};
+    // float R[4] = {VEL_MEASURE_NOISE, 0, 0, ACC_MEASURE_NOISE};
+    // float P[4] = {100000, 0, 0, 100000};
+    // float H[4] = {1, 0, 0, 1};
+    // memcpy(kf.F_data, F, sizeof(F));
+    // memcpy(kf.Q_data, Q, sizeof(Q));
+    // memcpy(kf.R_data, R, sizeof(R));
+    // memcpy(kf.P_data, P, sizeof(P));
+    // memcpy(kf.H_data, H, sizeof(H));
+}
+
+/**
+ * @brief 使用卡尔曼滤波估计底盘速度
+ * @todo 增加w和dw的滤波,当w和dw均小于一定值时,不考虑dw导致的角加速度
+ *
+ * @param lp 左侧腿
+ * @param rp 右侧腿
+ * @param cp 底盘
+ * @param imu imu数据
+ * @param delta_t 更新间隔
+ */
+void SpeedEstimation(LinkNPodParam *lp, LinkNPodParam *rp, ChassisParam *cp, INS_t *imu, float delta_t)
+{
+    // 修正轮速和距离
+    lp->wheel_w = lp->w_ecd + lp->phi2_w - cp->pitch_w; // 减去和定子固连的phi2_w
+    rp->wheel_w = rp->w_ecd + rp->phi2_w - cp->pitch_w;
+
+    // 直接使用轮速反馈,不进行速度融合
+    // cp->vel = (lp->wheel_w + rp->wheel_w) * WHEEL_RADIUS / 2;
+    // cp->dist = cp->dist + cp->vel * delta_t;
+
+    // 以轮子为基点,计算机体两侧髋关节处的速度
+    lp->body_v = lp->wheel_w * WHEEL_RADIUS + lp->leg_len * lp->theta_w + lp->legd * msin(lp->theta);
+    rp->body_v = rp->wheel_w * WHEEL_RADIUS + rp->leg_len * rp->theta_w + rp->legd * msin(rp->theta);
+    cp->vel_m = (lp->body_v + rp->body_v) / 2; // 机体速度(平动)为两侧速度的平均值
+
+    // 扣除旋转导致的向心加速度和角加速度*R
+    float *gyro = imu->Gyro, *dgyro = imu->dgyro;
+    static float yaw_ddwrNwwr, yaw_p_ddwrNwwr, pitch_ddwrNwwr;
+    static float macc_y, macc_z;                                // 补偿后的实际平动加速度,机体系前进方向和竖直方向
+    yaw_ddwrNwwr = powf(gyro[Z], 2) * CENTER_IMU_W - dgyro[Z] * CENTER_IMU_L;   // yaw旋转导致motion_acc[1]的额外加速度(机体前后方向)
+    yaw_p_ddwrNwwr = powf(gyro[X], 2) * CENTER_IMU_W + dgyro[X] * CENTER_IMU_H; // pitch旋转导致motion_acc[1]的额外加速度(机体前后方向)
+    pitch_ddwrNwwr = powf(gyro[X], 2) * CENTER_IMU_H - dgyro[X] * CENTER_IMU_W; // pitch旋转导致motion_acc[2]的额外加速度(机体竖直方向)
+    macc_y = -imu->MotionAccel_b[Y] - yaw_ddwrNwwr - yaw_p_ddwrNwwr;
+    macc_z = imu->MotionAccel_b[Z] - pitch_ddwrNwwr;
+
+    float pitch = imu->Pitch * DEGREE_2_RAD;
+    cp->acc_last = cp->acc_m; 
+    cp->acc_m = macc_y * mcos(pitch) - macc_z * msin(pitch); // 绝对系下的平动加速度,即机体系下的加速度投影到绝对系
+
+    // for debug 对比修正前后的加速度
+    static float ry, rz, rawaa; 
+    ry = -imu->MotionAccel_b[Y];
+    rz = imu->MotionAccel_b[Z];
+    rawaa = ry * mcos(pitch) - rz * msin(pitch);
+
+    // 使用kf同时估计加速度和速度,滤波更新
+    // kf.MeasuredVector[0] = cp->vel_m;
+    // kf.MeasuredVector[1] = cp->acc_m;
+    // kf.F_data[1] = delta_t; // 更新F矩阵
+    // Kalman_Filter_Update(&kf);
+    // cp->vel = kf.xhat_data[0];
+    // cp->acc = kf.xhat_data[1];
+
+    // 融合加速度计的数据和机体速度
+    static float f, k, prior, measure, cov;
+    f = (cp->acc_m + cp->acc_last) / 2;    // 速度梯形积分
+    prior = cp->vel + f * delta_t; // x' = Fx,先验估计
+    cp->vel_predict = prior;
+    measure = cp->vel_m;    // 测量值
+    cov = cp->vel_cov + VEL_PROCESS_NOISE * delta_t; // P' = P + Q ,先验协方差
+    cp->vel_cov = cov;
+    k = cov / (cov + VEL_MEASURE_NOISE);     // K = P'/(P'+R),卡尔曼增益
+    cp->vel = prior + k * (measure - prior); // x^ = x'+K(z-x'),后验估计
+    cp->vel_cov *= (1 - k);                  // P^ = (1-K)P',后验协方差
+    VAL_LIMIT(cp->vel_cov, 0.01, 100);       // 协方差限幅
+    cp->dist = cp->dist + cp->vel * delta_t;
+}
\ No newline at end of file
diff --git a/matlab/slprj/_jitprj/jitEngineAccessInfo.mat b/matlab/slprj/_jitprj/jitEngineAccessInfo.mat
index ed83d39..ffc3f06 100644
Binary files a/matlab/slprj/_jitprj/jitEngineAccessInfo.mat and b/matlab/slprj/_jitprj/jitEngineAccessInfo.mat differ
diff --git a/matlab/slprj/_jitprj/sX9KsJTz0Rxcyab0eSODvaB.l b/matlab/slprj/_jitprj/sX9KsJTz0Rxcyab0eSODvaB.l
new file mode 100644
index 0000000..1ca7ae1
Binary files /dev/null and b/matlab/slprj/_jitprj/sX9KsJTz0Rxcyab0eSODvaB.l differ
diff --git a/matlab/slprj/_jitprj/sX9KsJTz0Rxcyab0eSODvaB.mat b/matlab/slprj/_jitprj/sX9KsJTz0Rxcyab0eSODvaB.mat
new file mode 100644
index 0000000..7d79a60
Binary files /dev/null and b/matlab/slprj/_jitprj/sX9KsJTz0Rxcyab0eSODvaB.mat differ
diff --git a/matlab/slprj/_sfprj/EMLReport/emlReportAccessInfo.mat b/matlab/slprj/_sfprj/EMLReport/emlReportAccessInfo.mat
index 4440494..37eeef9 100644
Binary files a/matlab/slprj/_sfprj/EMLReport/emlReportAccessInfo.mat and b/matlab/slprj/_sfprj/EMLReport/emlReportAccessInfo.mat differ
diff --git a/matlab/slprj/_sfprj/EMLReport/sX9KsJTz0Rxcyab0eSODvaB.mat b/matlab/slprj/_sfprj/EMLReport/sX9KsJTz0Rxcyab0eSODvaB.mat
new file mode 100644
index 0000000..5f6fab5
Binary files /dev/null and b/matlab/slprj/_sfprj/EMLReport/sX9KsJTz0Rxcyab0eSODvaB.mat differ
diff --git a/matlab/slprj/_sfprj/precompile/D5ZxKRF0sEpdsk7hyWkDd.mat b/matlab/slprj/_sfprj/precompile/D5ZxKRF0sEpdsk7hyWkDd.mat
new file mode 100644
index 0000000..bb324b5
Binary files /dev/null and b/matlab/slprj/_sfprj/precompile/D5ZxKRF0sEpdsk7hyWkDd.mat differ
diff --git a/matlab/slprj/_sfprj/precompile/autoInferAccessInfo.mat b/matlab/slprj/_sfprj/precompile/autoInferAccessInfo.mat
index 213588a..5b87840 100644
Binary files a/matlab/slprj/_sfprj/precompile/autoInferAccessInfo.mat and b/matlab/slprj/_sfprj/precompile/autoInferAccessInfo.mat differ
diff --git a/matlab/slprj/_sfprj/precompile/hEenC4JTyow5SDrklRyRUF.mat b/matlab/slprj/_sfprj/precompile/hEenC4JTyow5SDrklRyRUF.mat
new file mode 100644
index 0000000..6e5f033
Binary files /dev/null and b/matlab/slprj/_sfprj/precompile/hEenC4JTyow5SDrklRyRUF.mat differ
diff --git a/matlab/slprj/_sfprj/wheel_leg/_self/sfun/info/binfo.mat b/matlab/slprj/_sfprj/wheel_leg/_self/sfun/info/binfo.mat
index 4251d76..caed502 100644
Binary files a/matlab/slprj/_sfprj/wheel_leg/_self/sfun/info/binfo.mat and b/matlab/slprj/_sfprj/wheel_leg/_self/sfun/info/binfo.mat differ
diff --git a/matlab/slprj/_sfprj/wheel_leg/amsi_serial.mat b/matlab/slprj/_sfprj/wheel_leg/amsi_serial.mat
index 57ad284..43684ab 100644
Binary files a/matlab/slprj/_sfprj/wheel_leg/amsi_serial.mat and b/matlab/slprj/_sfprj/wheel_leg/amsi_serial.mat differ
diff --git a/matlab/wheel_leg.slx b/matlab/wheel_leg.slx
index cd2716d..b9cc443 100644
Binary files a/matlab/wheel_leg.slx and b/matlab/wheel_leg.slx differ
diff --git a/modules/imu/ins_task.c b/modules/imu/ins_task.c
index ec084b6..ecb1f57 100644
--- a/modules/imu/ins_task.c
+++ b/modules/imu/ins_task.c
@@ -111,6 +111,7 @@ attitude_t *INS_Init(void)
 
     // noise of accel is relatively big and of high freq,thus lpf is used
     INS.AccelLPF = 0.0085;
+    INS.DGyroLPF = 0.008;
     DWT_GetDeltaT(&INS_DWT_Count);
     return (attitude_t *)&INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复.
 }
@@ -132,6 +133,9 @@ void INS_Task(void)
         INS.Accel[X] = BMI088.Accel[X];
         INS.Accel[Y] = BMI088.Accel[Y];
         INS.Accel[Z] = BMI088.Accel[Z];
+        INS.dgyro[X] = (BMI088.Gyro[X] - INS.Gyro[X])/ (INS.DGyroLPF + dt) + INS.dgyro[X] * INS.DGyroLPF / (INS.DGyroLPF + dt);
+        INS.dgyro[Y] = (BMI088.Gyro[Y] - INS.Gyro[Y])/ (INS.DGyroLPF + dt) + INS.dgyro[Y] * INS.DGyroLPF / (INS.DGyroLPF + dt);
+        INS.dgyro[Z] = (BMI088.Gyro[Z] - INS.Gyro[Z])/ (INS.DGyroLPF + dt) + INS.dgyro[Z] * INS.DGyroLPF / (INS.DGyroLPF + dt);
         INS.Gyro[X] = BMI088.Gyro[X];
         INS.Gyro[Y] = BMI088.Gyro[Y];
         INS.Gyro[Z] = BMI088.Gyro[Z];
diff --git a/modules/imu/ins_task.h b/modules/imu/ins_task.h
index 4acac19..0d4528c 100644
--- a/modules/imu/ins_task.h
+++ b/modules/imu/ins_task.h
@@ -56,6 +56,7 @@ typedef struct
 
     // IMU量测值
     float Gyro[3];  // 角速度
+    float dgyro[3]; 
     float Accel[3]; // 加速度
     // 位姿
     float Roll;