CAMILO ANDRES FLOREZ / Mbed OS KALMAN_LINEAL1

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Files at this revision

API Documentation at this revision

Revision 1:902778259e5b, committed 2019-10-09

Comitter:
cflorez75
Date:
Wed Oct 09 01:43:28 2019 +0000
Parent:
0:10cda550ba25
Commit message:
Filtro de Kalman Lineal

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed-os.lib Show annotated file Show diff for this revision Revisions of this file 
--- a/main.cpp	Mon Oct 07 23:50:04 2019 +0000
+++ b/main.cpp	Wed Oct 09 01:43:28 2019 +0000
@@ -8,7 +8,7 @@
 void Kalman(void);
 static volatile float PC11=1,PC12=0,PC21=0,PC22=1;
 static volatile float X1CN=0, X2CN=0;
-static volatile float V11=0.0001f, V22=0.0001f, W=0.0001f;
+static volatile float V11=0.000001f, V22=0.000001f, W=1000;
 
 
 int main(){
@@ -34,7 +34,7 @@
 DAC0->C1 = 0; 
 DAC0->C0 = DAC_C0_DACEN_MASK | DAC_C0_DACSWTRG_MASK| DAC_C0_DACRFS_MASK;
 
-bandera.attach_us(&Filtro,100);
+bandera.attach_us(&Filtro,50);
 
 while (1) { 
         } 
@@ -54,19 +54,19 @@
 u=((float)vA.read_u16())*33/65535;
 y=((float)iA.read_u16())*33/65535;
 
-X1PN1=0.0037f*u+0.9948f*X1CN-0.00001f*X2CN;
-X2PN1=0.0014f*u+0.7727f*X1CN+X2CN;
+X1PN1=0.003658303f*u+ 0.9948033528f*X1CN-0.000004789773312f*X2CN;
+X2PN1=0.001418225189f*u+0.7727265657f*X1CN+0.9999802845f*X2CN;
 
-PP11=0.98962704f*PC11-0.000009948f*PC12-0.000009948f*PC21+0.0000000001f*PC22+V11;
-PP12 = 0.76868196f*PC11+0.9948f*PC12-0.000007727f*PC21-0.00001f*PC22;
-PP21 = 0.76868196f*PC11-0.000007727f*PC12+0.9948f*PC21-0.00001f*PC22;
-PP22 = 0.5970652899f*PC11+0.7727f*PC12+0.7727f*PC21+PC22+V22;
+PP11 =0.9896337108f*PC11-0.00000476488f*PC12- 0.00000476488f*PC21+0.00000000002294192838f*PC22+0.9999999999f*V11;
+PP12=0.7687109781f*PC11+0.9947837396f*PC12- 0.00000370118f*PC21-0.00000478967f*PC22;
+PP21=0.7687109781f*PC11- 0.00000370118f*PC12 + 0.9947837394f*PC21- 0.00000478967f*PC22;
+PP22=0.5971063454f*PC11 + 0.7727113306f*PC12+ 0.7727113306f*PC21+ 0.9999605694f*PC22+ 0.9999999998f*V22;
 
-K11 = (9896270400*PP11 - 99480*PP12 - 99480*PP21 + PP22 + 10000000000*V11)/(9896270400*PP11 - 99480*PP12 - 99480*PP21 + PP22 + 10000000000*(V11 + W));
-K21 = 10*(768681960*PP11-7727*PP12+10000*(99480*PP21-PP22))/(9896270400*PP11 - 99480*PP12 - 99480*PP21 + PP22 + 10000000000*(V11 + W));
+K11= (PC11-0.000004814794096f*PC12-0.000004814794096f*PC21+0.0000000000231822422f*PC22+1.010474874f*V11)/(PC11-0.000004814794096f*PC12-0.000004814794096f*PC21+0.0000000000231822422f*PC22+1.010474874f*V11+1.010474874f*W);
+K21= (0.7767631292f*PC11-0.000003739954531f*PC12+1.005203974f*PC21-0.000004839850163f*PC22)/(PC11-0.000004814794096f*PC12-0.000004814794096f*PC21+0.0000000000231822422f*PC22+1.010474874f*V11+1.010474874f*W);
 
-D11=K11*(y-X2PN1);
-D21=K21*(y-X2PN1);
+D11=K11*(y-X1PN1);
+D21=K21*(y-X1PN1);
 
 X1CN1=X1PN1+D11;
 X2CN1=X2PN1+D21;
@@ -79,7 +79,7 @@
 X1CN=X1CN1;
 X2CN=X2CN1;
 
-yout=((uint16_t)(X2CN1*65535/330000));
+yout=((uint16_t)(X2CN1*65535/3300));
 
 DAC0->DAT[0].DATL = (uint8_t)(((uint16_t)yout>>4) & 0xFF);
 DAC0->DAT[0].DATH = (uint8_t)((((uint16_t)yout>>4) >> 8) & 0x0F);

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-os.lib	Wed Oct 09 01:43:28 2019 +0000
@@ -0,0 +1,1 @@
+https://github.com/ARMmbed/mbed-os/#cc7556a92fb9320f4bebb190c6e1315af116c50c