| View previous topic :: View next topic |
| Author |
Message |
eng.alamin
Joined: 08 Sep 2008
Posts: 17
|
| problem with( CONST float )declaration + KALMAN filter code |
 Posted: Sun Nov 29, 2009 2:20 pm |
 |
|
Hello
I have this IMU driver I wanted to implement in CCS but a list of
error 48 "Expecting ( "
error 43 : " Expecting a declaration"
Error 27 : "Expression must evaluate to a constant"
| Code: |
/*
* Please refer to tilt.c by Trammell Hudson <hudson@rotomotion.com>. Original code
* with extensive comments is available at http://www.rotomotion.com/downloads/tilt.c
*
* The State Update Routine is called every dt seconds with a new gyro rate measurement
* in degrees/second. Gyro reading and scaling is accomplished in a separete routine
* prior to calling state_update.
*
* The Kalman Update Routine is also called every dt seconds with a new angle measurement
* in degrees. The angle measurement is the difference between the desired balance angle and the
* angle derived from the accelerometer reading. Conversion and scaling of the accelerometer acceleration
* data to an angle measurement is accomplished in a separate routine prior to calling kalman_update.
*
* The accelerometer alone is not fast enough to balance the platform. The gyro is fast enough
* and the rate data can be integrated to calculate angle, however, bias or drift of the integrated
* gyro rate data over time makes them unusable on their own. The Kalman Filter fuses the
* accelerometer data with the gyro data to effectively unbias the integrated gyro rate data
* providing a better angle estimate than either the accelerometer or gyro on their own.
*/
#include <math.h>
static const float dt = 0.0205; //time in seconds between gyro rate measurements
#define R_angle_PARAM 0.3 //Adjusts Kalman filter convergence speed
#define Q_angle_PARAM 0.001 //Used in Q matix to set level of trust in accel
#define Q_gyro_PARAM 0.006 //data relative to gyro; originally 0.001 & 0.003
static float P[2][2] = { //Covariance matrix
{ 1, 0 },
{ 0, 1 } };
static float Pdot[4];
float angle = 0; //angle state
float q_bias = 0; //gyro bias state
float rate = 0; //unbiased angular rate
static const float R_angle = R_angle_PARAM;
static const float Q_angle = Q_angle_PARAM; //Q matrix
static const float Q_gyro = Q_gyro_PARAM;
//********************State Update Routine********************
//Called every dt seconds with a biased gyro measurement, it updates the
//current angle and rate estimate.
void state_update(float q_m) //gyro measurement
{
const 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********************
//Called every time a new accelometer measurement is available,
//in this implementation every dt seconds
float angle_err;
float C_0;
void kalman_update(float angle_m)
{
const float C_0 = 1;
/* const float C_1 = 0; */ //not used
const float PCt_0 = C_0 * P[0][0]; //+ C_1 * P[0][1] = 0
const float PCt_1 = C_0 * P[1][0]; //+ C_1 * P[1][1] = 0
const float E = R_angle + C_0 * PCt_0; //Compute error estimate
/* + C_1 * PCt_1 = 0 */ //not used
const float K_0 = PCt_0 / E; //Compute Kalman filter gains
const float K_1 = PCt_1 / E;
const float t_0 = PCt_0; //Compute measured angle
const 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;
}
|
SO ,, guys what the problem is how i can convert it to ccs ? |
|
 |
PCM programmer
Joined: 06 Sep 2003
Posts: 21708
|
|
| Posted: Sun Nov 29, 2009 2:40 pm |
|
|
| Do a search and replace, and delete 'const' in all the variable declarations. |
|
|
eng.alamin
Joined: 08 Sep 2008
Posts: 17
|
|
| Posted: Sun Nov 29, 2009 3:33 pm |
|
|
| thank you Sir |
|
|
|
FAQ 
Search
Register
Profile 
Log in to check your private messages 
Log in
