用卡尔曼互补滤波，为什么pitch这个值滤出来没有负数

float dt_pitch=0.005;//×¢Ò⣺dtµÄȡֵΪkalmanÂ˲¨Æ÷²ÉÑùʱ¼ä

float angle_kalmen_pitch, angle_dot_pitch;//½Ç¶ÈºÍ½ÇËÙ¶È

float P_pitch[2][2] = {{ 1, 0 },

              { 0, 1 }};

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

float Q_angle_pitch=0.001, Q_gyro_pitch=0.005; //½Ç¶ÈÊý¾ÝÖÃÐŶÈ,½ÇËÙ¶ÈÊý¾ÝÖÃÐŶÈ

float R_angle_pitch=0.1 ,C_0_pitch = 0.5; 

float q_bias_pitch=0, angle_err_pitch=0, PCt_0_pitch=0, PCt_1_pitch=0, E_pitch=0, K_0_pitch=0, K_1_pitch=0, t_0_pitch=0, t_1_pitch=0;



float Kalman_Filter_pitch(float angle_m, float gyro_m)//angleAx ºÍ gyroGy

{

                angle_kalmen_pitch += (gyro_m-q_bias_pitch) * dt_pitch;        //½ÇËٶȻý·Ö

                angle_err_pitch = angle_m - angle_kalmen_pitch;

        

                Pdot_pitch[0]=Q_angle_pitch - P_pitch[0][1] - P_pitch[1][0];

        

                Pdot_pitch[1]=-P_pitch[1][1];

                Pdot_pitch[2]=-P_pitch[1][1];

                Pdot_pitch[3]=Q_gyro_pitch;

        

                P_pitch[0][0] += Pdot_pitch[0] * dt_pitch;

                P_pitch[0][1] += Pdot_pitch[1] * dt_pitch;

                P_pitch[1][0] += Pdot_pitch[2] * dt_pitch;

                P_pitch[1][1] += Pdot_pitch[3] * dt_pitch;

        

                angle_err_pitch = angle_m - angle_kalmen_pitch;

        /******************************************************************/

        

                PCt_0_pitch = C_0_pitch * P_pitch[0][0];

                PCt_1_pitch = C_0_pitch * P_pitch[1][0];

        

                E_pitch = R_angle_pitch + C_0_pitch * PCt_0_pitch;

        

                K_0_pitch = PCt_0_pitch / E_pitch;

                K_1_pitch = PCt_1_pitch / E_pitch;

                

                t_0_pitch = PCt_0_pitch;

                

                t_1_pitch = C_0_pitch * P_pitch[0][1];

                

                P_pitch[0][0] -= K_0_pitch * t_0_pitch;

                P_pitch[0][1] -= K_0_pitch * t_1_pitch;

                P_pitch[1][0] -= K_1_pitch * t_0_pitch;

                P_pitch[1][1] -= K_1_pitch * t_1_pitch;

                

                angle_kalmen_pitch += K_0_pitch * angle_err_pitch; //×îÓŽǶÈ

                q_bias_pitch += K_1_pitch * angle_err_pitch;

                angle_dot_pitch = gyro_m-q_bias_pitch;//×îÓŽÇËÙ¶È

                return angle_kalmen_pitch;

}