Revision 0:e60a2d5cccc2, committed 2016-09-28

Comitter:

hirokimineshita

Date:

Wed Sep 28 11:12:37 2016 +0000

Commit message:

a

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/L3GD20.lib	Wed Sep 28 11:12:37 2016 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/hirokimineshita/code/L3GD20/#354deb9168c0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MMA7361.lib	Wed Sep 28 11:12:37 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/hirokimineshita/code/MMA7361/#c3d863eb7f91

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kalman.cpp	Wed Sep 28 11:12:37 2016 +0000
@@ -0,0 +1,1 @@
+/*if there is time I want to seperrate kalman.h to here*/
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kalman.h	Wed Sep 28 11:12:37 2016 +0000
@@ -0,0 +1,113 @@
+/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. 
+
+This software may be distributed and modified under the terms of the GNU 
+General Public License version 2 (GPL2) as published by the Free Software 
+Foundation and appearing in the file GPL2.TXT included in the packaging of 
+this file. Please note that GPL2 Section 2[b] requires that all works based 
+on this software must also be made publicly available under the terms of 
+the GPL2 ("Copyleft"). 
+
+ Contact information 
+ ------------------- 
+ 
+ Kristian Lauszus, TKJ Electronics 
+ Web      :  http://www.tkjelectronics.com 
+ e-mail   :  kristianl@tkjelectronics.com 
+ */ 
+
+#ifndef _Kalman_h 
+#define _Kalman_h 
+
+class Kalman {
+public:
+    Kalman() { 
+        /* We will set the variables like so, these can also be tuned by the user */ 
+        Q_angle = 0.001f; 
+        Q_bias = 0.003f; 
+        R_measure = 0.03f; 
+        
+        angle = 0.0f; // Reset the angle 
+        bias = 0.0f; // Reset bias 
+
+        P[0][0] = 0.0f; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical 
+        P[0][1] = 0.0f; 
+        P[1][0] = 0.0f; 
+        P[1][1] = 0.0f; 
+    }; 
+ 
+
+    // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds 
+    float getAngle(float newAngle, float newRate, float dt) { 
+        // KasBot V2  -  Kalman filter module - http://www.x-firm.com/?page_id=145 
+        // Modified by Kristian Lauszus 
+        // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it 
+
+        // Discrete Kalman filter time update equations - Time Update ("Predict") 
+        // Update xhat - Project the state ahead 
+        /* Step 1 */ 
+        rate = newRate - bias; 
+        angle += dt * rate; 
+
+        // Update estimation error covariance - Project the error covariance ahead 
+        /* Step 2 */ 
+        P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle); 
+        P[0][1] -= dt * P[1][1]; 
+        P[1][0] -= dt * P[1][1]; 
+        P[1][1] += Q_bias * dt; 
+
+        // Discrete Kalman filter measurement update equations - Measurement Update ("Correct") 
+        // Calculate Kalman gain - Compute the Kalman gain 
+        /* Step 4 */ 
+        float S = P[0][0] + R_measure; // Estimate error 
+        /* Step 5 */ 
+        float K[2]; // Kalman gain - This is a 2x1 vector 
+        K[0] = P[0][0] / S; 
+        K[1] = P[1][0] / S; 
+        
+        // Calculate angle and bias - Update estimate with measurement zk (newAngle) 
+        /* Step 3 */ 
+        float y = newAngle - angle; // Angle difference 
+        /* Step 6 */ 
+        angle += K[0] * y; 
+        bias += K[1] * y; 
+ 
+        // Calculate estimation error covariance - Update the error covariance 
+        /* Step 7 */ 
+        float P00_temp = P[0][0]; 
+        float P01_temp = P[0][1]; 
+
+        P[0][0] -= K[0] * P00_temp; 
+        P[0][1] -= K[0] * P01_temp; 
+        P[1][0] -= K[1] * P00_temp; 
+        P[1][1] -= K[1] * P01_temp; 
+
+
+        return angle; 
+    }; 
+
+    void setAngle(float newAngle) { angle = newAngle; }; // Used to set angle, this should be set as the starting angle 
+    float getRate() { return rate; }; // Return the unbiased rate 
+
+    /* These are used to tune the Kalman filter */ 
+    void setQangle(float newQ_angle) { Q_angle = newQ_angle; }; 
+    void setQbias(float newQ_bias) { Q_bias = newQ_bias; }; 
+    void setRmeasure(float newR_measure) { R_measure = newR_measure; }; 
+
+    float getQangle() { return Q_angle; }; 
+    float getQbias() { return Q_bias; }; 
+    float getRmeasure() { return R_measure; }; 
+
+private: 
+    /* Kalman filter variables */ 
+     float Q_angle; // Process noise variance for the accelerometer 
+     float Q_bias; // Process noise variance for the gyro bias 
+     float R_measure; // Measurement noise variance - this is actually the variance of the measurement noise
+     
+     float angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector 
+     float bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector 
+     float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate 
+ 
+     float P[2][2]; // Error covariance matrix - This is a 2x2 matrix 
+ }; 
+ 
+#endif 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Sep 28 11:12:37 2016 +0000
@@ -0,0 +1,76 @@
+#include "mbed.h"
+#include "l3gd20.h"
+#include "mma7361.h"
+#include "kalman.h"
+#include "binary.h"
+
+l3gd20 a;
+mma7361 b;
+Kalman c;
+DigitalOut myled(LED1);
+Serial pc(USBTX,USBRX);
+DigitalIn myb (USER_BUTTON);
+Timer t;
+
+int main() {
+    int us,d1,d2;
+    myled=1;
+    while(1) {
+        if(myb==0){
+            break;
+        }
+    }
+    
+    myled=0;
+    wait(1);
+    while(a.conect());
+    pc.printf("success\n\r");
+    a.s_write_8(CTRL_REG1,b0001111);
+    a.s_write_8(CTRL_REG5,b0000000);
+    b.clear_mat();
+    int x,z,fdata;
+    int16_t data;
+    b.set_1st_bit(&x,x_axis,0);
+    b.set_1st_bit(&z,z_axis,1);
+    float kx,gtheta,dt,atheta,xt,om,fom,iom;
+    c.setAngle(0);
+    atheta=0;
+    fdata=0;
+    fom=0;
+    iom=0;
+    gtheta=0;
+    while(1){
+        wait_ms(1);
+        d1=a.read_8(OUT_Z_H);
+        d2=a.read_8(OUT_Z_L);
+        data=(d1<<8)|d2;
+        b.low_pass_filter(&x,x_axis);
+        xt=b.bit_to_g(x,x_axis);
+        xt=xt*981/100;
+        om=sqrt(abs(xt*1000/37));
+        om=b.rad_to_deg(om);
+        if(om/data<0){
+            om*=-1;
+        }
+        iom=iom*0.9+om*0.1;
+        /*b.low_pass_filter(&z,z_axis);
+        atheta=b.rotate(&x,x_axis,&z,z_axis);
+        atheta=b.rad_to_deg(atheta);*/
+        t.stop();
+        us=t.read_us();
+        t.reset();
+        t.start();
+        dt=us*0.000001;
+        gtheta+=(data+fdata)*0.00875*dt*0.5;
+        atheta+=(om+fom)*dt*0.5;
+        /*if(atheta/kx<0 && abs(kx)>70){
+            atheta*=-1;
+        }*/
+        kx = c.getAngle(iom,gtheta,dt);
+        pc.printf("%.3f,%.3f,%d\n\r",kx,atheta,data);
+        //pc.printf("%.3f\n\r",om);
+        fdata=data;
+        fom=om;
+        gtheta=kx;
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Sep 28 11:12:37 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/4fc01daae5a5
\ No newline at end of file