Discussions

So, i am trying to use Kalman on a ADIS161400BMLZ to get my Attitude.

Everything works great when i hold my device in my hand and rotate it, but when i install it in my RC plane the Roll always shows 0 . When i start rolling and tuning the aircraft the roll shows 1-2 degrees and almost immediately returns to 0. It first seemed like it showed the rate of the roll instead of the roll itself but now it seems that is not the case because even when i roll the aircraft slowly the roll indicator returns to 0 before i stop rolling the aircraft.

Am struggling with this for a few days now.

Thanks for any help.

here is my code:

#include<math.h>

#include"uart_2.h"
#include "adis_16400.h"
#include "system.h"
#include"timers_init.h"
//#include"complementary_filter.h"
#include"Kalman_filter.h"
#include"main.h"
#include "calculations.h"
//======================== Extern var ==================

//=========================== Global var ==============================
/*
It var is data from registers of ADIS
*/

int XGYRO_OUT=0;
int YGYRO_OUT=0;
int ZGYRO_OUT=0;

int XGYRO_OFF=0;
int YGYRO_OFF=0;
int ZGYRO_OFF=0;

int XACCL_OUT=0;
int YACCL_OUT=0;
int ZACCL_OUT=0;

int XACCL_OFF=0;
int YACCL_OFF=0;
int ZACCL_OFF=0;

int XMAGN_OUT=0;
int YMAGN_OUT=0;
int ZMAGN_OUT=0;

int XMAGN_HIF=0;
int YMAGN_HIF=0;
int ZMAGN_HIF=0;

int TEMP_OUT=0;

char spiFlag=0;//for checking SPI
//================================================================
/*
Angles and rates after calculations
*/

float PitchAngle=0;
float PitchAngle_buf[10]={};
float PitchAngle_new=0.0;
float PitchRate=0.0;

float RollAngle=0;
float RollAngle_buf[10]={};
float RollAngle_new=0.0;
float RollRate=0.0;

float YawAngle=0;
float YawAngle_new=0.0;
float YawRate=0.0;

float Xacc=0.0;
float Yacc=0.0;
float Zacc=0.0;

float Xmagn=0.0;
float Ymagn=0.0;
float Zmagn=0.0;

float HeadingADIS=0;
float HeadingADIS_old=0;
float HeadingADIS_new=0.0;

float temperature=0;
//=====================================================================
/*
Offsets of ADIS
*/

float Xgyro_off=0;
float Ygyro_off=0;
float Zgyro_off=0;

float Xacc_off=0;
float Yacc_off=0;
float Zacc_off=0;

float Xmagn_hif=0;
float Ymagn_hif=0;
float Zmagn_hif=0;

//===========================================
//unsigned int PROD_ID=0;

/*
Scales of ADIS
*/

const float GyroScale=0.05;//0.025;
const float AccScale=3.33;
const float MagnScale=0.5;
const float TempScale=0.14;

//================================================

float HeadingADIS_buf[20]={};

void GetDataAdis()
{

//================== Gyro ==================

XGYRO_OUT=ReadADIS(0x0400);
YGYRO_OUT=ReadADIS(0x0600);
ZGYRO_OUT=ReadADIS(0x0800);
//sign extention
if(XGYRO_OUT & 0x2000) XGYRO_OUT=XGYRO_OUT | 0xC000;
else XGYRO_OUT=XGYRO_OUT & 0x3FFF;
if(YGYRO_OUT & 0x2000) YGYRO_OUT=YGYRO_OUT | 0xC000;
else YGYRO_OUT=YGYRO_OUT & 0x3FFF;
if(ZGYRO_OUT & 0x2000) ZGYRO_OUT=ZGYRO_OUT | 0xC000;
else ZGYRO_OUT=ZGYRO_OUT & 0x3FFF;

//============== Accelerometer ========================

XACCL_OUT=ReadADIS(0x0A00);
YACCL_OUT=ReadADIS(0x0C00);
ZACCL_OUT=ReadADIS(0x0E00);
//sign extention
if(XACCL_OUT & 0x2000) XACCL_OUT=XACCL_OUT | 0xC000;
else XACCL_OUT=XACCL_OUT & 0x3FFF;
if(YACCL_OUT & 0x2000) YACCL_OUT=YACCL_OUT | 0xC000;
else YACCL_OUT=YACCL_OUT & 0x3FFF;
if(ZACCL_OUT & 0x2000) ZACCL_OUT=ZACCL_OUT | 0xC000;
else ZACCL_OUT=ZACCL_OUT & 0x3FFF;

//=============== Magnitude ===================

XMAGN_OUT=ReadADIS(0x1000);
YMAGN_OUT=ReadADIS(0x1200);
ZMAGN_OUT=ReadADIS(0x1400);
//sign extention
if(XMAGN_OUT & 0x2000) XMAGN_OUT=XMAGN_OUT | 0xC000;
else XMAGN_OUT=XMAGN_OUT & 0x3FFF;
if(YMAGN_OUT & 0x2000) YMAGN_OUT=YMAGN_OUT | 0xC000;
else YMAGN_OUT=YMAGN_OUT & 0x3FFF;
if(ZMAGN_OUT & 0x2000) ZMAGN_OUT=ZMAGN_OUT | 0xC000;
else ZMAGN_OUT=ZMAGN_OUT & 0x3FFF;

//================= Temperature =================

TEMP_OUT=ReadADIS(0x1600);//12 bits is relevant
if(TEMP_OUT & 0x0800) TEMP_OUT=TEMP_OUT | 0xF000;
else TEMP_OUT=TEMP_OUT & 0x0FFF;
}

//================================================

void GetAnglesRates()//getting angles and rates from data of ADIS
{
float norm=0.0;

GetDataAdis();//========= Getting data from Adis16400

RollRate=(float)(XGYRO_OUT*GyroScale);//(float)XGYRO_OUT*GyroScale;//0.025===========???? 0.025 Datasheet => 0.05

PitchRate=-(float)(YGYRO_OUT*GyroScale);

YawRate=(float)(ZGYRO_OUT*GyroScale);//(float)ZGYRO_OUT*GyroScale;

//====================== Accelerometer =============

Xacc=(float)(XACCL_OUT*AccScale)+30.0;
Yacc=(float)(YACCL_OUT*AccScale);
Zacc=(float)(ZACCL_OUT*AccScale);

//================== Magnetometer ===============

Xmagn=(float)(XMAGN_OUT*MagnScale)+49.0;
Ymagn=(float)(YMAGN_OUT*MagnScale)-41.0;
Zmagn=(float)(ZMAGN_OUT*MagnScale);

norm= sqrtf( Xmagn*Xmagn + Ymagn*Ymagn + Zmagn*Zmagn);//normalization
//Xmagn=Xmagn/norm;
//Ymagn=Ymagn/norm;
//Zmagn=Zmagn/norm;


//================== Pitch angle ===================

if(Zacc!=0)
{
PitchAngle_new=(atan2(-Xacc,-Zacc)*180.0) /PI;
}
PitchAngle=Kalman_filter( PitchAngle,PitchAngle_new,PitchRate);
if(PitchAngle>180) PitchAngle=PitchAngle-360;

PitchAngle=MovingAverage(PitchAngle,PitchAngle_buf,10);

//================== Roll angle =====================

if(Zacc!=0)
{
RollAngle_new=(atan2(-Yacc,-Zacc)*180.0) /PI;
}
RollAngle=Kalman_filter( RollAngle,RollAngle_new,RollRate);
if(RollAngle>180) RollAngle=RollAngle-360;

RollAngle=MovingAverage(RollAngle,RollAngle_buf,10);

HeadingADIS=CalcHeadFromMagn(Xmagn,Ymagn,-Zmagn,PitchAngle,-RollAngle);

//===================== Smooth filter ================================

// if (fabsf(HeadingADIS - MovingAverage(HeadingADIS_old,HeadingADIS_buf,20)) > 20)
// {
// HeadingADIS = MovingAverage(HeadingADIS_old,HeadingADIS_buf,20);

// }

// HeadingADIS_old=CalcHeadFromMagn(Xmagn,Ymagn,Zmagn,PitchAngle,-RollAngle);

//===============================================================

temperature=(float)(TEMP_OUT*TempScale)+25.0;



}

int CalcHeadFromMagn(float x,float y,float z,int a,int b)
{
float Yh=0;
float Xh=0;
float Heading=0;

//find the SIN & COS of Pitch & Roll angles
float PitchCOS = cosf(a*PI /180.0);
float RollCOS = cosf(b*PI /180.0);
float PitchSIN = sinf(a*PI /180.0);
float RollSIN = sinf(b*PI /180.0);

x=x*PI/180.0;
y=y*PI/180.0;
z=z*PI/180.0;


if((a == 0) && (b == 0))
{
Xh = x; //Pitch & Roll = 0, therefore simpler X/Y calcs
Yh = y; //no Z axis value is required
}
else
{
//Xh (magnetic component) heading for arcTAN calc
//Xh = (x * PitchCOS) + (y * RollSIN * PitchSIN) - (z * RollCOS * PitchSIN);
//Yh (magnetic component) heading for arcTAN calc
//Yh = (y * RollCOS) + (z * RollSIN);

Xh = x * PitchCOS + z* PitchSIN;
Yh = x * RollSIN * PitchSIN + y * RollCOS - z * RollSIN * PitchCOS;
}

Heading =floor( (atan2f(Yh,Xh)*180.0 /PI)*10)/10+360;
//
// Xh = x * PitchCOS + z* PitchSIN;
// Yh = x * RollSIN * PitchSIN + y * RollCOS - z * RollSIN * PitchCOS;
//
// if (Xh < 0)
// Heading = PI - atanf(Yh / Xh);
// else if (Xh > 0 && Yh < 0)
// Heading = -atanf(Yh / Xh);
// else if (Xh > 0 && Yh > 0)
// Heading = PI * 2 - atan(Yh / Xh);
// else if (Xh == 0 && Yh < 0)
// Heading = PI / 2.0;
// else if (Xh == 0 && Yh > 0)
// Heading = PI * 1.5;
//
// Heading=Heading*180.0 /PI;
//Heading=floor( (atanf(Yh/Xh)*180.0 /PI)*10)/10;

if (Heading>360) Heading=Heading-360;
else if (Heading<0) Heading=Heading+360;

return Heading;
}

//===================================================

void GetOffsetAdis(void)
{
//================= Offset ==========================
XGYRO_OFF=ReadADIS(0x1A00);
YGYRO_OFF=ReadADIS(0x1C00);
ZGYRO_OFF=ReadADIS(0x1E00);
//sign extention
if(XGYRO_OFF & 0x2000) XGYRO_OFF=XGYRO_OFF | 0xC000;
else XGYRO_OFF=XGYRO_OFF & 0x3FFF;
if(YGYRO_OFF & 0x2000) YGYRO_OFF=YGYRO_OFF | 0xC000;
else YGYRO_OFF=YGYRO_OFF & 0x3FFF;
if(ZGYRO_OFF & 0x2000) ZGYRO_OFF=ZGYRO_OFF| 0xC000;
else ZGYRO_OFF=ZGYRO_OFF & 0x3FFF;

//===========================================
XACCL_OFF=ReadADIS(0x2000);
YACCL_OFF=ReadADIS(0x2200);
ZACCL_OFF=ReadADIS(0x2400);
//sign extention
if(XACCL_OFF & 0x2000) XACCL_OFF=XACCL_OFF | 0xC000;
else XACCL_OFF=XACCL_OFF & 0x3FFF;
if(YACCL_OFF & 0x2000) YACCL_OFF=YACCL_OFF | 0xC000;
else YACCL_OFF=YACCL_OFF & 0x3FFF;
if(ZACCL_OFF & 0x2000) ZACCL_OFF=ZACCL_OFF| 0xC000;
else ZACCL_OFF=ZACCL_OFF & 0x3FFF;

XMAGN_HIF=ReadADIS(0x2600);
YMAGN_HIF=ReadADIS(0x2800);
ZMAGN_HIF=ReadADIS(0x2A00);
//sign extention
if(XMAGN_HIF & 0x2000) XMAGN_HIF=XMAGN_HIF | 0xC000;
else XMAGN_HIF=XMAGN_HIF & 0x3FFF;
if(YMAGN_HIF & 0x2000) YMAGN_HIF=YMAGN_HIF | 0xC000;
else YMAGN_HIF=YMAGN_HIF & 0x3FFF;
if(ZMAGN_HIF & 0x2000) ZMAGN_HIF=ZMAGN_HIF| 0xC000;
else ZMAGN_HIF=ZMAGN_HIF & 0x3FFF;


Xgyro_off=XGYRO_OFF*GyroScale;
Ygyro_off=YGYRO_OFF*GyroScale;
Zgyro_off=ZGYRO_OFF*GyroScale;

Xacc_off=XACCL_OFF*AccScale;
Xacc_off=XACCL_OFF*AccScale;
Xacc_off=XACCL_OFF*AccScale;

Xmagn_hif=XMAGN_HIF*MagnScale;
Ymagn_hif=YMAGN_HIF*MagnScale;
Zmagn_hif=ZMAGN_HIF*MagnScale;

}
//==============================================

#include<stdio.h>
#include"Kalman_filter.h"

float Q_angle = 0.001; //0.0001
float Q_gyro = 0.5; //0.0003
float R_angle = 0.05; //0.003

float x_bias = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float y, S;
float K_0, K_1;

// newAngle = angle measured with atan2 using the accelerometer
// newRate = angle measured using the gyro
// looptime = loop time in millis()

float Kalman_filter(float Angle,float newAngle, float newRate)
{
float dt =0.2;//0.05; //0.2-200 ms;

Angle += dt * (newRate - x_bias);

P_00 += - dt * (P_10 + P_01) + Q_angle * dt;
P_01 += - dt * P_11;
P_10 += - dt * P_11;
P_11 += + Q_gyro * dt;

y = newAngle - Angle;

S = P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;

Angle += K_0 * y;

x_bias += K_1 * y;

P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;

return Angle;
}
//================================================