KalmanFilter C code error

[mohamedmostafamousa1991]

Posted on December 30, 2016 at 12:04

I am using stm32f discovery board. I am trying to fuse Acceleromoter and gyroscope using KF. And here is a code i included that suppose to do this.

#ifndef _Kalman_h
#define _Kalman_h
struct Kalman {
 /* Kalman filter variables */
 double Q_angle; // Process noise variance for the accelerometer
 double Q_bias; // Process noise variance for the gyro bias
 double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise
double angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector
double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector
double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
double P[2][2]; // Error covariance matrix - This is a 2x2 matrix
double K[2]; // Kalman gain - This is a 2x1 vector
double y; // Angle difference
double S; // Estimate error
};
void Init(struct Kalman* klm){
 /* We will set the variables like so, these can also be tuned by the user */
klm->Q_angle = 0.001;
klm->Q_bias = 0.003;
klm->R_measure = 0.03;
klm->angle = 0; // Reset the angle
klm->bias = 0; // Reset bias
klm->P[0][0] = 0; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - 
klm->P[0][1] = 0;
klm->P[1][0] = 0;
klm->P[1][1] = 0;
}
// The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
double getAngle(struct Kalman * klm, double newAngle, double newRate, double dt) {
 // Discrete Kalman filter time update equations - Time Update ('Predict')
 // Update xhat - Project the state ahead
 /* Step 1 */
 klm->rate = newRate - klm->bias;
 klm->angle += dt * klm->rate;
 // Update estimation error covariance - Project the error covariance ahead
 /* Step 2 */
 klm->P[0][0] += dt * (dt*klm->P[1][1] - klm->P[0][1] - klm->P[1][0] + klm->Q_angle);
 klm->P[0][1] -= dt * klm->P[1][1];
 klm->P[1][0] -= dt * klm->P[1][1];
 klm->P[1][1] += klm->Q_bias * dt;
 // Discrete Kalman filter measurement update equations - Measurement Update ('Correct')
 // Calculate Kalman gain - Compute the Kalman gain
 /* Step 4 */
 klm->S = klm->P[0][0] + klm->R_measure;
 /* Step 5 */
 klm->K[0] = klm->P[0][0] / klm->S;
 klm->K[1] = klm->P[1][0] / klm->S;
 // Calculate angle and bias - Update estimate with measurement zk (newAngle)
 /* Step 3 */
 klm->y = newAngle - klm->angle;
 /* Step 6 */
 klm->angle += klm->K[0] * klm->y;
 klm->bias += klm->K[1] * klm->y;
 // Calculate estimation error covariance - Update the error covariance
 /* Step 7 */
 klm->P[0][0] -= klm->K[0] * klm->P[0][0];
 klm->P[0][1] -= klm->K[0] * klm->P[0][1];
 klm->P[1][0] -= klm->K[1] * klm->P[0][0];
 klm->P[1][1] -= klm->K[1] * klm->P[0][1];
 return klm->angle;
}
void setAngle(struct Kalman* klm, double newAngle) { klm->angle = newAngle; } // Used to set angle, this should be set as the starting angle
double getRate(struct Kalman* klm) { return klm->rate; } // Return the unbiased rate
/* These are used to tune the Kalman filter */
void setQangle(struct Kalman* klm, double newQ_angle) { klm->Q_angle = newQ_angle; }
void setQbias(struct Kalman* klm, double newQ_bias) { klm->Q_bias = newQ_bias; }
void setRmeasure(struct Kalman* klm, double newR_measure) { klm->R_measure = newR_measure; }
double getQangle(struct Kalman* klm) { return klm->Q_angle; }
double getQbias(struct Kalman* klm) { return klm->Q_bias; }
double getRmeasure(struct Kalman* klm) { return klm->R_measure; }
#endif�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?�?

Now when using only the getAngle function, it always retunrs nan. i kept searching for the problem for 2 days until i found that whatever i do this function always returns a nan. So i will be glad if someone tell me what might be the problem.

[Seb]

Posted on December 30, 2016 at 12:30

Is it necessary to put struct within functions? Never do.

double getQangle(struct Kalman* klm) { return klm->Q_angle; }

<=>

double getQangle(Kalman* klm) { return klm->Q_angle; }

Somewhere you need to define at least a global variable for your data...

#ifndef _Kalman_h
#define _Kalman_h
struct Kalman {
 /* Kalman filter variables */
 double Q_angle; // Process noise variance for the accelerometer
 double Q_bias; // Process noise variance for the gyro bias
 double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise
Kalman KSingle;
Kalman KMultiple[10];
double angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector
double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector
double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate

In your main, either use:

getAngle(&Ksingle, 0, 0, 0);
or
getAngle(&KMultiple[3], 0, 0, 0);

No other clues than above to try.

Good luck!

[mohamedmostafamousa1991]

Posted on December 30, 2016 at 12:59

Thanks for your reply. Actually the function i am talking about is the 2nd one, the getangle not getQangle. all other functions are working.

[Seb]

Posted on December 30, 2016 at 13:30

How about debugging/investigate the issue?

==> Let's disable most of the code in the function below it and simplify the debug. Find and error tracking...

insert 

return 0;

on top of the function, compile and test.

Move the return few lines down, rinse and repeat until the culprit is found.

Also make sure the declaration of the function matches its definition. Most compilers such as IAR will generate an error or warning.

[mohamedmostafamousa1991]

Posted on December 30, 2016 at 13:46

I did all debugging i can. and the problem is with this one klm->angle. IDK y its reading it here as an nan eventhough it's working in the other function

[mohamedmostafamousa1991]

Posted on December 31, 2016 at 02:16

I solved the problem