智能车制作

标题: 新手求教，卡尔曼出来的最优值总是比传感器值小，调啥？ [打印本页]

作者: 3038922 时间: 2017-3-21 18:06
标题: 新手求教，卡尔曼出来的最优值总是比传感器值小，调啥？
新手求教，算出来的最优值总是比传感器值小，调啥？我是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 ( ) ;

欢迎光临智能车制作 (http://dns.znczz.com/)