wangine
Joined: 07 Jul 2009
Posts: 98
Location: Curtea de Arges, Romania
|
| Kalman filter (simplified) |
 Posted: Tue Nov 03, 2015 7:57 pm |
 |
|
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
https://youtu.be/77iTKJu4Y5k?si=BoVLf2jgXJ5V_FzJ
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.
Last edited by wangine on Sun Aug 18, 2024 6:04 pm; edited 1 time in total |
|
FAQ 
Search
Register
Profile 
Log in to check your private messages 
Log in
