CAMILO ANDRES FLOREZ
/
KALMAN_LINEAL1
Archivo que contiene filtro de kalman lineal con periodo de muestreo de 50us
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 |
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