View Single Post
  #26   Spotlight this post!  
Unread 16-04-2012, 09:10
JamesTerm's Avatar
JamesTerm JamesTerm is offline
Terminator
AKA: James Killian
FRC #3481 (Bronc Botz)
Team Role: Engineer
 
Join Date: May 2011
Rookie Year: 2010
Location: San Antonio, Texas
Posts: 298
JamesTerm is a splendid one to beholdJamesTerm is a splendid one to beholdJamesTerm is a splendid one to beholdJamesTerm is a splendid one to beholdJamesTerm is a splendid one to beholdJamesTerm is a splendid one to beholdJamesTerm is a splendid one to behold
Re: High Speed Encoder Problem

Here is the Kalman Filter:

Code:
  /***********************************************************************************************************/
 /*												KalmanFilter												*/
/***********************************************************************************************************/

void KalmanFilter::Reset()
{
	m_FirstRun=true;
    //initial values for the kalman filter
    m_x_est_last = 0.0;
    m_last = 0.0;
}

KalmanFilter::KalmanFilter(): m_Q(0.022),m_R(0.617)  //setup Q and R as the noise in the system
{
}

double KalmanFilter::operator()(double input)
{
	//For first run set the last value to the measured value
	if (m_FirstRun)
	{
		m_x_est_last=input;
		m_FirstRun=false;
	}
    //do a prediction
    double x_temp_est = m_x_est_last;
    double P_temp = m_last + m_Q;
    //calculate the Kalman gain
    double K = P_temp * (1.0/(P_temp + m_R));
    //the 'noisy' value we measured
    double z_measured = input;
    //correct
    double x_est = x_temp_est + K * (z_measured - x_temp_est); 
    double P = (1- K) * P_temp;
    
    //update our last's
    m_last = P;
    m_x_est_last = x_est;
    
	//Test for NAN
	if ((!(m_x_est_last>0.0)) && (!(m_x_est_last<0.0)))
		m_x_est_last=0;

    return x_est;
}