Gerrit Pathuis / Mbed 2 deprecated K22F_DSP_Matrix_least_square

Example of a least square caculation, Matrix calculation based on CMSIS-DSP library, x(t)=a+b*t+c*t^2.

Dependencies:   dsp mbed

Comment out line 226 of "arm_math.h" to prevent a lot of compiler warnings

main.cpp@0:9087ea0ad689, 2015-03-10 (annotated)

Committer:
GerritPathuis
Date:
Tue Mar 10 12:14:31 2015 +0000
Revision:
0:9087ea0ad689
Program works

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GerritPathuis 0:9087ea0ad689 1 ///////////////////////////////////////
GerritPathuis 0:9087ea0ad689 2 // FRDM-K22F //
GerritPathuis 0:9087ea0ad689 3 // Least square calculation (Matrix) //
GerritPathuis 0:9087ea0ad689 4 // Based on the DSP libtary //
GerritPathuis 0:9087ea0ad689 5 // Digital Signal Processing //
GerritPathuis 0:9087ea0ad689 6 // //
GerritPathuis 0:9087ea0ad689 7 // Results can be read via Tera Term //
GerritPathuis 0:9087ea0ad689 8 ///////////////////////////////////////
GerritPathuis 0:9087ea0ad689 9 #include "mbed.h"
GerritPathuis 0:9087ea0ad689 10 #include "arm_math.h"
GerritPathuis 0:9087ea0ad689 11
GerritPathuis 0:9087ea0ad689 12 #define NUMSAMPLES 51
GerritPathuis 0:9087ea0ad689 13 #define NUMUNKNOWS 3
GerritPathuis 0:9087ea0ad689 14
GerritPathuis 0:9087ea0ad689 15 Serial pc(USBTX, USBRX); // tx, rx
GerritPathuis 0:9087ea0ad689 16
GerritPathuis 0:9087ea0ad689 17 // time is evenly spaced bit this in not required
GerritPathuis 0:9087ea0ad689 18 float32_t tData[NUMSAMPLES] = {
GerritPathuis 0:9087ea0ad689 19 0.0f, 0.1f, 0.2f, 0.3f, 0.4f, 0.5f, 0.6f, 0.7f,
GerritPathuis 0:9087ea0ad689 20 0.8f, 0.9f, 1.0f, 1.1f, 1.2f, 1.3f, 1.4f, 1.5f,
GerritPathuis 0:9087ea0ad689 21 1.6f, 1.7f, 1.8f, 1.9f, 2.0f, 2.1f, 2.2f, 2.3f,
GerritPathuis 0:9087ea0ad689 22 2.4f, 2.5f, 2.6f, 2.7f, 2.8f, 2.9f, 3.0f, 3.1f,
GerritPathuis 0:9087ea0ad689 23 3.2f, 3.3f, 3.4f, 3.5f, 3.6f, 3.7f, 3.8f, 3.9f,
GerritPathuis 0:9087ea0ad689 24 4.0f, 4.1f, 4.2f, 4.3f, 4.4f, 4.5f, 4.6f, 4.7f,
GerritPathuis 0:9087ea0ad689 25 4.8f, 4.9f, 5.0f
GerritPathuis 0:9087ea0ad689 26 };
GerritPathuis 0:9087ea0ad689 27
GerritPathuis 0:9087ea0ad689 28 float32_t xData[NUMSAMPLES] = {
GerritPathuis 0:9087ea0ad689 29 07.4213f, 21.7231f, -7.2828f, 21.2254f, 20.2221f, 10.3585f, 20.2033f, 29.2690f,
GerritPathuis 0:9087ea0ad689 30 57.7152f, 53.6075f, 22.8209f, 59.8714f, 43.1712f, 38.4436f, 46.0499f, 39.8803f,
GerritPathuis 0:9087ea0ad689 31 41.5188f, 55.2256f, 55.1803f, 55.6495f, 49.8920f, 34.8721f, 50.0859f, 57.0099f,
GerritPathuis 0:9087ea0ad689 32 47.3032f, 50.8975f, 47.4671f, 38.0605f, 41.4790f, 31.2737f, 42.9272f, 24.6954f,
GerritPathuis 0:9087ea0ad689 33 23.1770f, 22.9120f, 3.2977f, 35.6270f, 23.7935f, 12.0286f, 25.7104f, -2.4601f,
GerritPathuis 0:9087ea0ad689 34 06.7021f, 01.6804f, 02.0671f, -2.2891f, -16.2070f, -14.2204f, -20.1780f, -18.9303f,
GerritPathuis 0:9087ea0ad689 35 -20.4859f, -25.8338f, -47.2892f
GerritPathuis 0:9087ea0ad689 36 };
GerritPathuis 0:9087ea0ad689 37
GerritPathuis 0:9087ea0ad689 38
GerritPathuis 0:9087ea0ad689 39
GerritPathuis 0:9087ea0ad689 40 float32_t AData[NUMSAMPLES * NUMUNKNOWS];
GerritPathuis 0:9087ea0ad689 41 float32_t ATData[NUMSAMPLES * NUMUNKNOWS];
GerritPathuis 0:9087ea0ad689 42 float32_t ATAData[NUMUNKNOWS * NUMUNKNOWS];
GerritPathuis 0:9087ea0ad689 43 float32_t invATAData[NUMUNKNOWS * NUMUNKNOWS];
GerritPathuis 0:9087ea0ad689 44 float32_t BData[NUMSAMPLES * NUMSAMPLES];
GerritPathuis 0:9087ea0ad689 45 float32_t cData[NUMUNKNOWS];
GerritPathuis 0:9087ea0ad689 46
GerritPathuis 0:9087ea0ad689 47 arm_matrix_instance_f32 t= {NUMSAMPLES, 1, tData};
GerritPathuis 0:9087ea0ad689 48 arm_matrix_instance_f32 x= {NUMSAMPLES, 1, xData};
GerritPathuis 0:9087ea0ad689 49 arm_matrix_instance_f32 A= {NUMSAMPLES, NUMUNKNOWS, AData};
GerritPathuis 0:9087ea0ad689 50 arm_matrix_instance_f32 AT= {NUMUNKNOWS, NUMSAMPLES, ATData};
GerritPathuis 0:9087ea0ad689 51 arm_matrix_instance_f32 ATA= {NUMUNKNOWS, NUMUNKNOWS, ATAData};
GerritPathuis 0:9087ea0ad689 52 arm_matrix_instance_f32 invATA= {NUMUNKNOWS, NUMUNKNOWS, invATAData};
GerritPathuis 0:9087ea0ad689 53 arm_matrix_instance_f32 B= {NUMUNKNOWS, NUMSAMPLES, BData};
GerritPathuis 0:9087ea0ad689 54 arm_matrix_instance_f32 c= {NUMUNKNOWS, 1, cData};
GerritPathuis 0:9087ea0ad689 55
GerritPathuis 0:9087ea0ad689 56 main(void)
GerritPathuis 0:9087ea0ad689 57 {
GerritPathuis 0:9087ea0ad689 58 int i, time;
GerritPathuis 0:9087ea0ad689 59 float y;
GerritPathuis 0:9087ea0ad689 60
GerritPathuis 0:9087ea0ad689 61 pc.printf("/n/nMatrix calculation (Least squares) with 3 Unknowns \n\r");
GerritPathuis 0:9087ea0ad689 62 pc.printf("We calculates a, b and c from; x(t)= a + b*t + c*t*t \n\r");
GerritPathuis 0:9087ea0ad689 63 pc.printf("Results approx a= 8.701225, b= 38.880058, c= -9.792965\n\r");
GerritPathuis 0:9087ea0ad689 64
GerritPathuis 0:9087ea0ad689 65 y= sqrtf(xData[0]);
GerritPathuis 0:9087ea0ad689 66 cData[0]= y;
GerritPathuis 0:9087ea0ad689 67
GerritPathuis 0:9087ea0ad689 68 for(i=0; i<NUMSAMPLES; i++) {
GerritPathuis 0:9087ea0ad689 69 AData[i*NUMUNKNOWS +0] = 1.0f;
GerritPathuis 0:9087ea0ad689 70 AData[i*NUMUNKNOWS +1] = tData[i];
GerritPathuis 0:9087ea0ad689 71 AData[i*NUMUNKNOWS +2] = tData[i] * tData[i];
GerritPathuis 0:9087ea0ad689 72 }
GerritPathuis 0:9087ea0ad689 73
GerritPathuis 0:9087ea0ad689 74 /* calculation of A transpose */
GerritPathuis 0:9087ea0ad689 75 arm_mat_trans_f32(&A, &AT);
GerritPathuis 0:9087ea0ad689 76 arm_mat_mult_f32(&AT, &A, &ATA);
GerritPathuis 0:9087ea0ad689 77 arm_mat_inverse_f32(&ATA, &invATA);
GerritPathuis 0:9087ea0ad689 78 arm_mat_mult_f32(&invATA, &AT, &B);
GerritPathuis 0:9087ea0ad689 79 arm_mat_mult_f32(&B, &x, &c);
GerritPathuis 0:9087ea0ad689 80
GerritPathuis 0:9087ea0ad689 81
GerritPathuis 0:9087ea0ad689 82 pc.printf("Results a= %f, b= %f, c= %f \n\r", cData[0], cData[1], cData[2]);
GerritPathuis 0:9087ea0ad689 83
GerritPathuis 0:9087ea0ad689 84 for(time=0; time<6; time++) {
GerritPathuis 0:9087ea0ad689 85 y= cData[0] + cData[1]* time + cData[2] * time * time;
GerritPathuis 0:9087ea0ad689 86 pc.printf("time= %d, x= %f \n\r", time, y );
GerritPathuis 0:9087ea0ad689 87 }
GerritPathuis 0:9087ea0ad689 88 }