Archivo que contiene filtro de kalman lineal con periodo de muestreo de 50us

Files at this revision

API Documentation at this revision

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
diff -r 10cda550ba25 -r 902778259e5b main.cpp
--- 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);
diff -r 10cda550ba25 -r 902778259e5b mbed-os.lib
--- /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