新手求教，卡尔曼出来的最优值总是比传感器值小，调啥？

3038922

新手求教，算出来的最优值总是比传感器值小，调啥？我是C写的，跟论坛里的代码似乎有点不同。我好想木有调传感器读取响应时间那行代码。也不知道怎么加。
我的破传感器响应速度慢，本来是希望他预测下 下步数值，结果每次都比传感器数值要小。请教下是调哪个参数啊？
谢谢各位啦。
红字是我自己弄的卡尔曼，可能不太对。参数也是瞎调的。
#include "Main.h"
void pid_encoder_FR ( long target, long tolerance, unsigned long time )
{
      float kp = 0.55;
      float ki = 0.007;
      float kd = 5;
      int insidetarget = 0;
      int error = 0;
      int integeral = 0;
      int derivative = 0;
      int lasterror = 0;
      unsigned long time1 = 0;
      unsigned long time2 = 0;
      float m_xLast = 0; // 洗蜗低匙优值
      float m_xMid; // 旨系统子疟
      float m_xNow; // 本次系统子胖?
      float m_pLast = 1; // 洗蝐ovariance值
      float m_pMid; // 旨ovariance?
      float m_pNow; // 本次covariance值
      float m_kg;
      float nMersure = 0;
      float m_R = 0.5;
      float m_Q = 1; // 系统?
      PresetIntegratedMotorEncoder ( 9 , 0 ) ;
      StartTimer ( 3 ) ;
      PresetTimer ( 3 , 0 ) ;
      StartTimer ( 4 ) ;
      PresetTimer ( 4 , 0 ) ;
      while ( (insidetarget == 0 || time1 < 400) && time2 <= time )
      {
            time1 = GetTimer ( 3 ) ;
            time2 = GetTimer ( 4 ) ;
            m_xMid=m_xLast ; // //将弦淮蔚南低匙优赋值敢桓旨?
            m_pMid=m_pLast+m_Q ; // 将弦淮蔚腸ovariance值加舷低吃（m_Q）赋值敢桓旨保ㄏ低嘲自在牡试的钩讨 换着备持担??对不能是，在不断的彩怨程帜改崭值，得到囊?淖姬梗?
            m_kg=m_pMid/(m_pMid+m_R) ; // 计薻g的公式
            nMersure = GetIntegratedMotorEncoder ( 9 ) ;
            m_xNow=m_xMid+m_kg*(nMersure-m_xMid) ; // 通辜扑得到沾蔚南低匙优nMersure为系统?
            m_pNow=(1-m_kg)*m_pMid ; // 计说鼻暗腸ovariance值
            m_xLast=m_xNow ; // 嘎系统子?
            m_pLast=m_pNow ; // 嘎covariance值
            PrintToScreen ( "m_xNow:%f\n" , m_xNow ) ;
            PrintToScreen ( "nMersure:%f\n" , nMersure ) ;
            error= target-m_xNow ;
            if ( abs(error)<=tolerance )
            {
                  insidetarget=1 ;
            }
            else
            {
                  insidetarget=0 ;
                  PresetTimer ( 1 , 0 ) ;
            }
            if ( abs(error)<50 && (error != 0) )
            {
                  integeral = integeral + error ;
            }
            else
            {
                  integeral =0 ;
            }
            derivative = error- lasterror  ;
            lasterror = error ;
            output=(int)((float)error*kp)+((float)integeral*ki)+((float)derivative*kd);
            if ( output >127 )
            {
                  output=127 ;
            }
            if ( output <-127 )
            {
                  output=-127 ;
            }
            motor_drive_FR ( ) ;
            Wait ( 10 ) ;
      }
      PrintToScreen ( "pid_encoder_FR:%d\n" , (int)nMersure ) ;
      PrintToScreen ( "pid_encoder_FR_time:%d\n" , (long)time2 ) ;
      StopTimer ( 1 ) ;
      StopTimer ( 2 ) ;
      motor_driver_stop ( ) ;