我在做平衡小车，在卡尔曼滤波计算出倾角之后我再进行PID调节的时候，因为在PID调节函数里面加上了if(Angle>-3.0&&Angle<3.0){Angle=0.0;}，然后发现倾角在我缓慢变化至3度后，刚过3度时倾角会突然猛的增加，然后减小最后稳定在正常值上，如果我不加这句话，倾角变化一直正常，百思不得其解，希望有大神能帮帮我。。。。

另外，卡尔曼滤波和速度PID控制我都是写在中断里的，周期5ms，PID参数我暂时只加了一个P调节，其他均为零。

//******卡尔曼参数************

                  

const float Q_angle=0.001;  

const float Q_gyro=0.003;

const float R_angle=0.5;

const float dt=0.07;                           //dt为kalman滤波器采样时间;或0.005,0.07

const char C_0 = 1;

float Q_bias=0.0, Angle_err=0.0;

float PCt_0=0, PCt_1=0, E=0;

float K_0=0, K_1=0, t_0=0, t_1=0;

float Pdot[4] ={0,0,0,0};

float PP[2][2] = { { 1, 0 },{ 0, 1 } };

float position;

static const float Kp  = 40.0;                             //PID参数  Angle  

static const float Kd  = 0.0;                                         //PID参数  Gyro_y  

static const float Ksp = 0.0;                                 //PID参数  motor_speed  

static const float Ksi = 0.0;                                  //PID参数    position         

  

/*****************卡尔曼滤波**************************************************/

void Kalman_Filter(float Accel,float Gyro)               

{

         Angle+=(Gyro- Q_bias) * dt; //先验估计

         

         Pdot[0]=Q_angle- PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分

         Pdot[1]=-PP[1][1];

         Pdot[2]=-PP[1][1];

         Pdot[3]=Q_gyro;

         

         PP[0][0]+= Pdot[0] * dt;   // Pk-先验估计误差协方差微分的积分

         PP[0][1]+= Pdot[1] * dt;   // =先验估计误差协方差

         PP[1][0]+= Pdot[2] * dt;

         PP[1][1]+= Pdot[3] * dt;

         

         Angle_err= Accel - Angle;        //zk-先验估计

         

         PCt_0= C_0 * PP[0][0];

         PCt_1= C_0 * PP[1][0];

         

         E= R_angle + C_0 * PCt_0;

         

         K_0= PCt_0 / E;

         K_1= PCt_1 / E;

         

         t_0= PCt_0;

         t_1= C_0 * PP[0][1];

         PP[0][0]-= K_0 * t_0;                  //后验估计误差协方差

         PP[0][1]-= K_0 * t_1;

         PP[1][0]-= K_1 * t_0;

         PP[1][1]-= K_1 * t_1;

                  

         Angle        += K_0 * Angle_err;           //后验估计

         Q_bias      += K_1 * Angle_err;           //后验估计

         Gyro_y   = Gyro - Q_bias;       //输出值(后验估计)的微分=角速度

}

void Angle_Calculate(void)

{

/****************************加速度****************************************/

         

         Accel_x  = MPU6050_Real_Data.Accel_X;           //读取X轴加速度

         Angle_ax= Accel_x*1.2*180/3.14;     //弧度转换为度

  

/****************************角速度****************************************/

         

          Gyro_y = MPU6050_Real_Data.Gyro_Y;            

   Angle_gy += (Gyro_y)*0.003;

         

/***************************卡尔曼融合*************************************/

         Kalman_Filter(Angle_ax,Angle_gy);       //卡尔曼滤波计算倾角

         

}

void Speed_Pid_Calculate(void)

{

           if(Angle>-3.0&&Angle<3.0){Angle=0.0;}

                  

                   Speed_control  = (Kp*Angle + Kd*Gyro_y);            //PID：角速度和角度调节

                 Speed_control +=(Ksp*motor_speed + Ksi*position);   

                  

                   //PID：车速度调节

                  

           speed_control_l= (int)Speed_control ;            //左轮速度

                   speed_control_r= (int)- Speed_control;    //右轮速度

                  

                   if(speed_control_l> MAX_SPEED) speed_control_l = MAX_SPEED;

                   if(speed_control_l< -MAX_SPEED) speed_control_l = -MAX_SPEED;

                   if(speed_control_r> MAX_SPEED) speed_control_r = MAX_SPEED;

                   if(speed_control_r< -MAX_SPEED) speed_control_r = -MAX_SPEED;

}

1

2

3

4

5

楼主希望的是个线性函数变化，但是显然是个非线性的变化，你的公式计算对吗？