To all Kalman's Filter pro!

[leo123]

Im using a PIC 16F877A and MPLAB pro v7.61 compiler. Been working on my final year project which is a balancing robot. All the hardware and software seems to work fine but im having problems with the kalman filter. The LCD keeps displaying 0.00degrees. The kalman code that im using is from trammell hudson's 1DOF kalman filter. Help really needed as my project deadline is coming!

#include <stdio.h>
#include <htc.h>
#include <math.h>
#include "lcd.h"
#include "delay.h"

__CONFIG(0x3f71);

static const float dt = 0.02;

static float P[2][2] = {
{ 1, 0 },
{ 0, 1 },
};

float angle;
float q_bias;
float rate;

static const float R_angle = 0.3;

static const float Q_angle = 0.001;
static const float Q_gyro = 0.003;


void
state_update(
float q_m /* Pitch gyro measurement */
)
{
float q = q_m - q_bias;

float Pdot[2 * 2];

Pdot[0] = Q_angle - P[0][1] - P[1][0]; /* 0,0 */
Pdot[1] = - P[1][1]; /* 0,1 */
Pdot[2] = - P[1][1]; /* 1,0 */
Pdot[3] = Q_gyro; /* 1,1 */

rate = q;
angle += q * dt;

P[0][0] += Pdot[0] * dt;
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;
}

void
kalman_update(
float ax_m, /* X acceleration */
float az_m /* Z acceleration */
)
{
float angle_m = atan2( -az_m, ax_m );
float angle_err = angle_m - angle;

const float C_0 = 1;

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;

float K_0 = PCt_0 / E;
float K_1 = PCt_1 / E;

float t_0 = PCt_0; /* C_0 * P[0][0] + C_1 * P[1][0] */
float t_1 = C_0 * P[0][1]; /* + C_1 * P[1][1] = 0 */

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 += K_0 * angle_err;
q_bias += K_1 * angle_err;
}

void main ( void )
{
char LCDString[ 17 ];
unsigned int adcValue[7];
float accDegreeX[ 3 ],accDegreeY[ 3 ],gyroRateX[ 3 ];
unsigned char tmp;

// Initializations
lcd_init();

lcd_goto ( 0x00 );//Printing Kalman's Filter on the top LCD
lcd_puts ( "Kalman's Filter" );
lcd_goto ( 0x40 );//Printing Degree: on the bottom LCD
lcd_puts ( "Degree:" );

PORTB = 0x00;

TRISA = 0xff;
TRISD = 0x00;
ADCON1 = 0xc8;//Selecting type of ADC pins

while ( 1 )
{
// Body
ADCON0 = 0x81;
ADGO = 1;
while ( ADGO )
continue;
adcValue [ 0 ] = ( ADRESH << 8 ) | ADRESL;//Getting ADC from RA0(gyroRateX)

ADCON0 = 0x89;
ADGO = 1;
while ( ADGO )
continue;
adcValue [ 1 ] = ( ADRESH << 8 ) | ADRESL;//Getting ADC from RA1(gyroRateY)

ADCON0 = 0xa1;
ADGO = 1;
while ( ADGO )
continue;
adcValue [ 2 ] = ( ADRESH << 8 ) | ADRESL;//Getting ADC from RA5

ADCON0 = 0xa9;
ADGO = 1;
while ( ADGO )
continue;
adcValue [ 3 ] = ( ADRESH << 8 ) | ADRESL;//Getting ADC from RE0(accDegreeX)

ADCON0 = 0xb1;
ADGO = 1;
while ( ADGO )
continue;
adcValue [ 4 ] = ( ADRESH << 8 ) | ADRESL;//Getting ADC from RE1(accDegreeY)

ADCON0 = 0xb9;
ADGO = 1;
while ( ADGO )
continue;
adcValue [ 5 ] = ( ADRESH << 8 ) | ADRESL;//Getting ADC from RE2(accDegreeZ)

gyroRateX[ 0 ] = (adcValue[ 0 ]*5.0)/1023;
gyroRateX[ 1 ] = (gyroRateX[ 0 ]-1.5)/0.002;//Basic calculation to get d/dt for gyroRateX

accDegreeX[ 0 ] = (adcValue[ 3 ]*5.0)/1023;
accDegreeX[ 1 ] = (accDegreeX[ 0 ]-1.6)/0.3;//Basic calculation to get g for accDegreeX

accDegreeY[ 0 ] = (adcValue[ 4 ]*5.0)/1023;
accDegreeY[ 1 ] = (accDegreeY[ 0 ]-1.6)/0.3;//Basic calculation to get g for accDegreeY

state_update ( gyroRateX [ 1 ] );
kalman_update ( accDegreeX [ 1 ], accDegreeY [ 1 ] );

lcd_goto ( 0x48 );
tmp = ( unsigned char ) angle;
angle = ( angle - tmp ) * 10;
lcd_putch ( tmp + '0' );
lcd_putch ( '.' );
tmp = ( unsigned char ) angle;
angle = ( angle - tmp ) * 10;
lcd_putch ( tmp + '0' );
tmp = ( unsigned char ) angle;
angle = (angle - tmp ) * 10;
lcd_putch ( tmp + '0' );
lcd_putch ( 0xdf );
}
}

[Archangel]

Hi Leo,
You DID notice this is Not a " C " forum, so help in " C " might be a little slow in coming.