Extra information of the Kalman Filter

[Josh Rendon]

Hello all,

Here's the paper on the intro to the Kalman filter that I was talking
about last meeting.
http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html

The code used to just draw raw data from the accelerometer and
gyroscope has been posted online in the file "Sensors.pde". You can
open this file in a text editor, or modify it in the Arduino IDE.

I hope to see you all at the meeting tonight, we will be having our
elections for next semester tonight.

Thanks,
Josh Rendon
Team Head

[Josh Rendon]

Apparently they removed our ability to upload files to Google Groups.
In order to circumvent this issue the code is posted bellow:

/*This program written to use both an accelerometer (ADXL335) and a
* gyroscope (IDG500) to determine the inclination angles of an object.
*
*/

/* Pin settings (for my configuration)
* Vin <->3.3v
* Gnd <-> Gnd
* Ax <-> 0
* Ay <-> 1
* Az <-> 2
* Gx <-> 3
* Gy <-> 0
*/

#define PI 3.14159265358979f
int printedTitleFlag = 0; //determines if the title has been printed
//same for gyro and Acc
float Vref = 5;
int analogADCRes = 1023; //1023 resolution for 10-bit adc



//Accelerometer variables
int accInPins[3] = {0,1,2}; //input pins for accelerometer {x,y,z}
float accSensitivity = .3; //300 mv/g

float accZeroGVolt[3] = {1.79,1.76,1.86}; //the zero voltages are
taken experimentally using the 1-point calibration technique
float Raccl=0;
float AdcRacc[3] = {0,0,0}; //the input data read from ADC
float Racc[3]={0,0,0}; //{x,y,z} in g
float vAcc[3]={0,0,0}; //this is the analog input converted to volts
float accAngle[3]={0,0,0}; //these are the angles relative to the x,
y, and z axes
//they are theta, psi, phi

//Gyro Variables
int gyroInPins[2] = {3,4}; //input pins for gyro {x,y,z}
float gyroSensitivity = 0.002; //2.0 mV/deg/sec

float gyroZeroGVolt[2] = {1.50,1.50}; //taken experimentally using
the 1-point calibration technique
float AdcRgyro[2] = {0,0}; //the input data read from ADC
float gyroRate[2]={0,0}; //{x,y} in deg/s
float gyroAngle[2]={0,0};
float vGyro[2]={0,0}; //this is the analog input converted to volts
unsigned long lastMicros;
unsigned long interval;

void setup(){
Serial.begin(9600);
}

void loop(){

getSensorData();
// convertToVoltsAcc();
//convertToVoltsGyro();
// printSensorDataVolts();
convertToGacc(); //Note: You must convert the data to G's before
calculating the angles
convertToAngleAcc();
convertToRateGyro();
convertToAngleGyro();
printSensorDataAngles();


}

void printSensorDataVolts(){
if(printedTitleFlag == 0){
Serial.println("vX\tvY\tvZ\t||\tvGyro\tvGyro");
printedTitleFlag = 1;
}
Serial.print(vAcc[0]);
Serial.print("\t");
Serial.print(vAcc[1]);
Serial.print("\t");
Serial.print(vAcc[2]);
Serial.print("\t");
Serial.print("||");
Serial.print("\t");
Serial.print(vGyro[0]);
Serial.print("\t");
Serial.print(vGyro[1]);
Serial.println();
}

void printSensorDataAngles(){
if(printedTitleFlag == 0){
Serial.println("AccX\tAccY\tAccZ\t||\tGyroX\tGyroY");
printedTitleFlag = 1;
}
Serial.print(accAngle[0]);
Serial.print("\t");
Serial.print(accAngle[1]);
Serial.print("\t");
Serial.print(accAngle[2]);
Serial.print("\t");
Serial.print("||");
Serial.print("\t");
Serial.print(gyroRate[0]);
Serial.print("\t");
Serial.print(gyroRate[1]);
Serial.println();
}

void printElements(float array[]){

int size = sizeof(array);

for(int i=0; i<size; i++){
Serial.print(array[i]);

if(i != (size-1))
Serial.print("\t");
}
Serial.println();
}

//This method inputs the adc data from both the acc and the gyro,
then averages them respectively
void getSensorData(){
static unsigned long newMicros; //new timestamp
//<average> the values to remove some error
int loopCount = 12;

for(int i = 0; i< loopCount; ++i)
{
// It takes 100 us (0.0001 s) to read an analog input
AdcRacc[0] += analogRead(accInPins[0]);
AdcRacc[1] += analogRead(accInPins[1]);
AdcRacc[2] += analogRead(accInPins[2]);

AdcRgyro[0] += analogRead(gyroInPins[0]);
AdcRgyro[1] += analogRead(gyroInPins[1]);

}
newMicros = micros(); //save the time when sample is taken
interval = newMicros - lastMicros; //please note that overflows
are ok, since for example 0x0001 - 0x00FE will be equal to 2
lastMicros = newMicros; //save for next loop, please
note interval will be invalid in first sample but we don't use it

AdcRacc[0] /= loopCount;
AdcRacc[1] /= loopCount;
AdcRacc[2] /= loopCount;

AdcRgyro[0] /= loopCount;
AdcRgyro[1] /= loopCount;
//</end average>

}

//Accelerometer methods
void convertToAngleAcc(){
//this is the technique shown in the adxl335 data sheet (this is
the technique that should be used)
accAngle[0] = atan(Racc[0]/sqrt(pow(Racc[1],2) + pow(Racc[2],
2)))*180/PI;
accAngle[1] = atan(Racc[1]/sqrt(pow(Racc[0],2) + pow(Racc[2],
2)))*180/PI;
accAngle[2] = atan(sqrt(pow(Racc[0],2) + pow(Racc[1],2)) /
Racc[2])*180/PI;
}

void convertToVoltsAcc(){
vAcc[0] = (AdcRacc[0] * Vref/analogADCRes);
vAcc[1] = (AdcRacc[1] * Vref/analogADCRes);
vAcc[2] = (AdcRacc[2] * Vref/analogADCRes);
}


void getSensorDataAcc(){
//Read in the analog inputs in (mv/g)
//<average> the values to remove some error
int loopCount = 12;

for(int i = 0; i< loopCount; ++i)
{
// It takes 100 us (0.0001 s) to read an analog input
AdcRacc[0] += analogRead(accInPins[0]);
AdcRacc[1] += analogRead(accInPins[1]);
AdcRacc[2] += analogRead(accInPins[2]);
delay(10);
}
AdcRacc[0] /= loopCount;
AdcRacc[1] /= loopCount;
AdcRacc[2] /= loopCount;

//</end average>
}

void convertToGacc(){
// Complete formula:
//Rx = ((AdcRx * Vref /analogADCRes) - zeroGVoltX) / sensitivity

//NEw code uses one Point calibration technique first
Racc[0] = ((AdcRacc[0] * Vref/analogADCRes) - accZeroGVolt[0]) /
accSensitivity; // V (g/V) = V
Racc[1] = ((AdcRacc[1] * Vref/analogADCRes) - accZeroGVolt[1]) /
accSensitivity;
Racc[2] = ((AdcRacc[2] * Vref/analogADCRes) - accZeroGVolt[2]) /
accSensitivity;
}

/*
void convertToAngleAlternate(){
Racc = sqrt(pow(R[0],2) + pow(R[1],2) + pow(R[2],2));
accAngles[0] = acos(R[0]/Racc)*180/PI;
accAngles[1] = acos(R[1]/Racc)*180/PI;
angles[2] = acos(R[2]/Racc)*180/PI;

}
*/


//Gyro methods
void convertToVoltsGyro(){
vGyro[0] = (AdcRgyro[0] * Vref/analogADCRes);
vGyro[1] = (AdcRgyro[1] * Vref/analogADCRes);
}

void getSensorDataGyro(){
//Read in the analog inputs in (mv/g)
//<average> the values to remove some error
int loopCount = 12;

for(int i = 0; i< loopCount; ++i)
{
// It takes 100 us (0.0001 s) to read an analog input
AdcRgyro[0] += analogRead(gyroInPins[0]);
AdcRgyro[1] += analogRead(gyroInPins[1]);
delay(10);
}
AdcRgyro[0] /= loopCount;
AdcRgyro[1] /= loopCount;
//</end average>
}

void convertToRateGyro(){
// Complete formula:
//RateAxz = ((AdcGyroXZ * Vref) / 1023 – VzeroRate) / Sensitivity

//NEw code uses one Point calibration technique first
gyroRate[0] = ((AdcRgyro[0] * Vref/analogADCRes) -
gyroZeroGVolt[0]) / gyroSensitivity; // V (g/V) = V
gyroRate[1] = ((AdcRgyro[1] * Vref/analogADCRes) -
gyroZeroGVolt[1]) / gyroSensitivity;
}

void convertToAngleGyro(){
gyroAngle[0] = gyroRate[0]*interval;
gyroAngle[1] = gyroRate[1] *interval;

}



On Apr 19, 7:29 pm, Josh Rendon <josh.ren...@gmail.com> wrote:
> Hello all,
>
> Here's the paper on the intro to the Kalman filter that I was talking

> about last meeting.http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html