CCS C Software and Maintenance Offers
FAQFAQ   FAQForum Help   FAQOfficial CCS Support   SearchSearch  RegisterRegister 

ProfileProfile   Log in to check your private messagesLog in to check your private messages   Log inLog in 

CCS does not monitor this forum on a regular basis.

Please do not post bug reports on this forum. Send them to support@ccsinfo.com

Kalman filter (simplified)

 
Post new topic   Reply to topic    CCS Forum Index -> Code Library
View previous topic :: View next topic  
Author Message
wangine



Joined: 07 Jul 2009
Posts: 98
Location: Curtea de Arges, Romania

View user's profile Send private message Send e-mail Yahoo Messenger

Kalman filter (simplified)
PostPosted: Tue Nov 03, 2015 7:57 pm     Reply with quote

Long time ago i just spent 6 - 8 months to find and write a simple fast and precise kalman filter to fit in low PIC18 family. After some months i put in a dsPIC33F family and with days and days past, made a milion comparisons on scope, a analizer, PC, matlab, i dont remember really. I optimize the filter, after i research the math of kalman, comparing other C and C++ kalman filters, to decide with is the best choice.
I will put here my filter because i search the forum for a Differential Evolution algorithm, also kalman for curiosity and i didn't find any library, just some comments.
To explain simple, how accurate is chosen the update filter, knowing a frecqency processor, the reading time of each device (in my case the acc and gyro) than accurate the filter will be. Normally the
Code:
float dt = 0.00003680f;
variable should be a constant, but in my case was updated in other functions after each ADC read, with a timer help.
Code:

/*********************************************************************
  Writer Tanase Bogdan
  (c) copyright www.wangine.ro
  ALL RIGHTS RESERVED
 
********************************************************************
 Ver 0.6 release 29/iunie/2011
********************************************************************/

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~ m  ,u  ,p  //
 float dt = 0.00003680f;       //time in seconds 0.0000368f with single gyro axis0.000246f;
//static float dt = 0.00016605;       //time in seconds  aprox measure of my scope 0.00016605;
//static float dt = 0.0002372;       //time in seconds  aprox measure of my scope
//static float dt = 0.0002802;       //time in seconds  aprox measure of my scope

#define      R_angle_PARAM      0.3      //03 how match trust to acc

#define      Q_angle_PARAM      0.001  // 0.001 //0.0009f angle parameter update every increase "dt"
#define      Q_gyro_PARAM      0.002   //data relative to gyro; originally 0.001 & 0.003 //0.0016
//      >>>> how match trust to gyro

static float   P[2][2] = {            //Covariance matrix
                    { 1, 0 },
                      { 0, 1 } };
           
static float Pdot[4];

float         angle = 0.0f;            //angle state //98.20f
float         q_bias = 470;            //gyro bias state 4492 ,, 936 read dupa init, nou 470
float         rate = 0;            //unbiased angular rate //0.83f

 static float   R_angle   = R_angle_PARAM;
 static float   Q_angle   = Q_angle_PARAM;   //Q matrix
 static float   Q_gyro   = Q_gyro_PARAM;

//********************State Update Routine********************
//Called every dt seconds with a biased gyro measurement, it updates the , no need bias gyro
//current angle and rate estimate.

void  state_update(float q_m)                 //gyro measurement
{

   if(angle <= -0.006 ^ angle <= 0.006)
   {   dt += 0.00001597f; }   // stabilize to angle

   if(angle <= -0.02 ^ angle <= 0.02)
   {   dt += 0.00000897f; }   // stabilize to angle



    float q = q_m - q_bias;            //unbias gyro measurement

   Pdot[0] = Q_angle - P[0][1] - P[1][0];      //Compute the derivative of the covariance matrix
   Pdot[1] = - P[1][1];
   Pdot[2] = - P[1][1];
   Pdot[3] = Q_gyro;
   
   rate = q;                           //Save unbiased gyro estimate

   angle += q * dt;                     //Update angle estimate

   P[0][0] += Pdot[0] * dt;               //Update covariance matirx
   P[0][1] += Pdot[1] * dt;
   P[1][0] += Pdot[2] * dt;
   P[1][1] += Pdot[3] * dt;
}


//********************Kalman Update Routine********************
//Chemata de cate ori data de la acc este transferata
// incrementeaza la fiecare dt

float      angle_err;
float      C_0;

void kalman_update(float angle_m)
{
    float C_0 = 1;
   /* const float      C_1 = 0; */            //not used
   
    float      PCt_0 = C_0 * P[0][0];      //+ C_1 * P[0][1] = 0
    float      PCt_1 = C_0 * P[1][0];      //+ C_1 * P[1][1] = 0
   
    float      E = R_angle + C_0 * PCt_0;   //Compute error estimate
   /*   + C_1 * PCt_1 = 0 */               //not used
   
    float      K_0 = PCt_0 / E;         //Compute Kalman filter gains
    float      K_1 = PCt_1 / E;
   
    float      t_0 = PCt_0;            //Compute measured angle
    float      t_1 = C_0 * P[0][1];      //and error in the estimate

   P[0][0] -= K_0 * t_0;
   P[0][1] -= K_0 * t_1;
   P[1][0] -= K_1 * t_0;
   P[1][1] -= K_1 * t_1;

   angle_err = angle_m - angle;
   
   angle   += K_0 * angle_err ;               //Update state estimate
   q_bias   += K_1 * angle_err ;          //Update bias estimate  not necesary
}

For math explanation here http://math.stackexchange.com/questions/840662/an-explanation-of-the-kalman-filter or other simply google search.
and here in action
http://www.220.ro/documentare/Self-Balancing-Robot/umHTJRCLns/
I want to mention: On first attempts during first 2 months, trying many filters, my robot with same battery ran only 6-10 min. After some months and learning hard the math of kalman my robot was able to run more than 3 hours continuously.
wangine



Joined: 07 Jul 2009
Posts: 98
Location: Curtea de Arges, Romania

View user's profile Send private message Send e-mail Yahoo Messenger

PostPosted: Thu Nov 05, 2015 11:43 am     Reply with quote

AND My research comes from :
http://www.mymathlib.com/quadrature/rectangle_rule.html
http://www.mymathlib.com/quadrature/simpsons_rule.html
https://github.com/shimniok/diyrovers/blob/master/DataBus/Estimation/kalman.cpp
http://www.mymathlib.com/quadrature/rombergs_method.html
http://www.seattlerobotics.org/encoder/200311/brown/building_a_directional_gyro.html
http://www.chiefdelphi.com/forums/showthread.php?t=39537&page=3
http://www.instructables.com/id/ArduinoGyroscopeProcessing/
one of the best source
http://www.cs.unc.edu/~welch/kalman/
one milions sources for inspiration
https://sites.google.com/site/onewheeledselfbalancing/Home/links-to-other-self-balancing-projects
http://www.starlino.com/imu_guide.html
http://forum.arduino.cc/index.php/topic,8652.0.html
http://aeroquad.com/content.php?s=382bef36515168450968e0389c3c5193
http://www.microchip.com/forums/tm.aspx?m=127772&high=constrain
http://www.advancedsourcecode.com/gaborfiltering.asp[/b]
and many many others , and take so many time just to compare and read all the information.
Display posts from previous:   
Post new topic   Reply to topic    CCS Forum Index -> Code Library All times are GMT - 6 Hours
Page 1 of 1

 
Jump to:  
You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot vote in polls in this forum


Powered by phpBB © 2001, 2005 phpBB Group