To all PRO kalman's filter!

[leo123]

im programming a PIC16F877A using MPLAB pro v7.61. The only problem is my lcd only display 0.00degree. That means that there is a problem with my kalman filter. Help iss really needed as my 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 );
}
}

[Thenero]

I have no idea about how a kalman would work but I did find something odd.

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 );

or more specificly

angle - tmp 
tmp + '0' 

I think you want to do some mathimatic changes to angle.
But now you are adding and substracting to the value of the char
For example if tmp = 3 the result would be 99 in the second example.
That does not appear to be intended behavior to me unless lcd_putch is aware of it.

[m4ster r0shi]

Are you sure your port initialization is done properly? Did you write the algorithm yourself or did you copy it? If it's the latter, it's more likely you didn't set your ports up properly before running the algorithm.

Thenero wrote:

I think you want to do some mathimatic changes to angle.

No, he just prints it there. Check this out:

#include <iostream>
using namespace std;

void lcd_putch(unsigned char c)
{
    cout << c << flush;
}

int main()
{
    float angle=1.23;
    unsigned char tmp;

    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' );

    cout << "\nhit enter to quit...";
    cin.get();
    return 0;
}

[Thenero]

No, he just prints it there. Check this out:

Ow now I see yes he is acting like the char is a number and then uses the +'0' to push it to
the correct place in the ASCII table before printing. I should have seen that.