Claudio Donate
Program for testing the Kalman Filter. Receives input from gyro and accelerometer and oututs PITCH and ROLL.
Dependencies: mbed kalman ITG3200 BMA180
Revision 0:384bc42ad0cf, committed 2012-08-15
- Comitter:
- cdonate
- Date:
- Wed Aug 15 22:20:07 2012 +0000
- Commit message:
- Program for testing the Kalman Filter
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BMA180.lib Wed Aug 15 22:20:07 2012 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/cdonate/code/BMA180/#c7eb9f15e026
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ITG3200.lib Wed Aug 15 22:20:07 2012 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/cdonate/code/ITG3200/#044664f2cc01
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/kalman.lib Wed Aug 15 22:20:07 2012 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/cdonate/code/kalman/#8a460b0d6f09
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Aug 15 22:20:07 2012 +0000
@@ -0,0 +1,82 @@
+#include "mbed.h"
+#include "kalman.c"
+#include "BMA180.h"
+#include "ITG3200.h"
+
+#define PI 3.1415926535897932384626433832795
+#define Rad2Dree 57.295779513082320876798154814105
+
+Serial pc(USBTX, USBRX);
+
+I2C I2CBus(p28, p27);
+
+Timer GlobalTime;
+Timer ProgramTimer;
+
+float R;
+double angle[3];
+unsigned long timer;
+long loopStartTime;
+
+BMA180 Acc(I2CBus, GlobalTime);
+ITG3200 Gyro(I2CBus, GlobalTime);
+kalman filter_pitch;
+kalman filter_roll;
+
+int main() {
+ int count = 0;
+ pc.baud(115200);
+
+ I2CBus.frequency(400000);
+ GlobalTime.start();
+
+ Acc.Init();
+ wait_ms(500);
+
+ //User Calibration
+ short Raw1g[3]= {0, 0, 0};
+ Acc.userCalibration(Raw1g);
+
+ //0.5s Calibration
+ Acc.Calibrate(500, Raw1g);
+
+ Gyro.Init();
+ wait_ms(500);
+
+ //0.5s Calibration
+ Gyro.Calibrate(500);
+
+ // Parameters ( R_angle, Q_gyro, Q_angle )
+ kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
+ kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
+
+
+ ProgramTimer.start();
+ loopStartTime = ProgramTimer.read_us();
+ timer = loopStartTime;
+
+ while(1) {
+
+ //Aquire new values for the Gyro and Acc
+ Acc.Update();
+ Gyro.Update();
+
+ //Calcuate the resulting vector R from the 3 acc axes
+ R = sqrt(std::pow(Acc.Acc[0] , 2) + std::pow(Acc.Acc[1] , 2) + std::pow(Acc.Acc[2] , 2));
+
+ kalman_predict(&filter_pitch, Gyro.Rate[0], (ProgramTimer.read_us() - timer));
+ kalman_update(&filter_pitch, acos(Acc.Acc[0]/R));
+ kalman_predict(&filter_roll, Gyro.Rate[1], (ProgramTimer.read_us() - timer));
+ kalman_update(&filter_roll, acos(Acc.Acc[1]/R));
+
+ angle[0] = kalman_get_angle(&filter_pitch);
+ angle[1] = kalman_get_angle(&filter_roll);
+
+ timer = ProgramTimer.read_us();
+
+ //count++;
+ //if(count == 10000){pc.printf("Program time: %i\n\r", ProgramTimer.read_us() - loopStartTime);loopStartTime = ProgramTimer.read_us(); count = 0;}
+ pc.printf("Pitch Angle X: %.6f Pitch Angle Y: %.6f \n\r", Rad2Dree * angle[1], Rad2Dree * angle[0]);
+
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Aug 15 22:20:07 2012 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/10b9abbe79a6 \ No newline at end of file