应用于两轮平衡车上的卡尔曼滤波程序

痛苦的滋味

struct kalman
{
        float        KalmanFilterGain;                  //卡尔曼增益
        float        CurrentEstimateValue;        //当前估计值
        float         CurrentEstimateCovariance;        //当前估计协方差
        float        CurrentMeasureCovariance;        //当前测量协方差
}Kalman_Filter;

void InitialKalmanFilter()
{
        Kalman_Filter.CurrentEstimateValue = 0;
        Kalman_Filter.CurrentEstimateCovariance = 0.1;
        Kalman_Filter.CurrentMeasureCovariance = 0.03;
}

float KalmanFilter(float InputValue)
{
        Kalman_Filter.KalmanFilterGain = Kalman_Filter.CurrentEstimateCovariance *
                                                                         sqrt(1 / (Kalman_Filter.CurrentEstimateCovariance * Kalman_Filter.CurrentEstimateCovariance +
                                                                         Kalman_Filter.CurrentMeasureCovariance * Kalman_Filter.CurrentMeasureCovariance));
        Kalman_Filter.CurrentEstimateValue = Kalman_Filter.CurrentEstimateValue + Kalman_Filter.KalmanFilterGain * (InputValue -
                                                                                 Kalman_Filter.CurrentEstimateValue);
        Kalman_Filter.CurrentEstimateCovariance = sqrt(1 - Kalman_Filter.KalmanFilterGain) * Kalman_Filter.CurrentEstimateCovariance;
        Kalman_Filter.CurrentMeasureCovariance = sqrt(1 - Kalman_Filter.KalmanFilterGain) * Kalman_Filter.CurrentMeasureCovariance;
        
        return Kalman_Filter.CurrentEstimateValue;               
}以上是小弟我写的一个卡尔曼滤波程序，主要是用在SISO系统中也就是我要把这段函数写到我的两轮平衡车里面实现角度的卡尔曼滤波，不知是否正确，InputValue就是传感器采集到的角度值。请各位大虾帮小弟看看。