0

I had a code that implements Kalman noise filtering on Arduino, and there is a use of malloc() and free(). I'm wondering if this will affect the Arduino or not (for example, the program will not function as I wanted, ...).

This is because when I got all raw (unfiltered) data and wrote the same Kalman Noise Filter function on Matlab, the filtering works well. However, when I write this FKalman function directly in Arduino, it doesn't work like it did on Matlab.

The Kalman Noise Filter is based on this article: http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/

Here is the code (just have a look at the function Fkalman and the tick, ignore the other parts):

#define START_MSG

#define NBDATASEND 6
#define NBBYTE2SEND (2*NBDATASEND)

char StartMessage[] = "START";
char StopMessage[] = "STOP";
uint8_t byteRead;
uint8_t StartAsked = 0;
uint8_t StopAsked = 0;
unsigned long previous = 0;
int mod_11k = 12; 
int mod_17k = 13; 
uint8_t Header = 174;
uint8_t tab_1[NBBYTE2SEND]={0,0,0,0,0,0,0,0,0,0,0,0};
uint16_t data_toSend[NBDATASEND]={0,0,0,0,0,0};

float P1[4];float P2[4];float P3[4]; float P4[4];float P5[4];float P6[4];
float xprec1[2];float xprec2[2];float xprec3[2];float xprec4[2];float xprec5[2];float xprec6[2];
float *sol1;float *sol2;float *sol3; float *sol4;float *sol5;float *sol6;
float Te;
float Tbeg;

void setup() {
  pinMode(mod_11k,OUTPUT);  
  pinMode(mod_17k,OUTPUT);
  digitalWrite(mod_11k,LOW);
  digitalWrite(mod_17k,LOW);
  //Start serial port with Gumstix 
  Serial1.begin(115200);

//Initialize KF
 P1[0] = 0.05; P1[1] =0; P1[2] =0; P1[3] = 0.05;
 P2[0] = 0.05; P2[1] =0; P2[2] =0; P2[3] = 0.05;
 P3[0] = 0.05; P3[1] =0; P3[2] =0; P3[3] = 0.05;
 P4[0] = 0.05; P4[1] =0; P4[2] =0; P4[3] = 0.05;
 P5[0] = 0.05; P5[1] =0; P5[2] =0; P5[3] = 0.05;
 P6[0] = 0.05; P6[1] =0; P6[2] =0; P6[3] = 0.05;
 xprec1[0] =0; xprec1[1] =0;
 xprec2[0] =0; xprec2[1] =0;
 xprec3[0] =0; xprec3[1] =0;
 xprec4[0] =0; xprec4[1] =0;
 xprec5[0] =0; xprec5[1] =0;
 xprec6[0] =0; xprec6[1] =0;

  //Wait for start message
#ifdef START_MSG
  //Waiting START message to begin
  StartAsked = 0;
  while (StartAsked == 0)
  {
    if (Serial1.available() >= strlen(StartMessage))        //?????
    {
      //Serial.write("Bytes available");
      // verifiy that the received stream is well the start stream
      for (i = 0; i < strlen(StartMessage); i++)
      {
        byteRead = Serial1.read();
        if ((uint8_t)StartMessage[i] == byteRead)
        {
          StartAsked = 1;
        }
       // Serial.println(byteRead);
      }
    }
    delay(1);
  }
  Serial1.write("START");
  //Serial.write("START");
#endif

//KF first time
    Tbeg = millis();
    digitalWrite(mod_11k,HIGH);
    delayMicroseconds(2000);
    data_toSend[0] = analogRead(0);   //phi1 11k
    data_toSend[1] = analogRead(1); //phi2 11k
    data_toSend[2] = analogRead(2);
    digitalWrite(mod_11k,LOW);
    delayMicroseconds(200);
    digitalWrite(mod_17k,HIGH);
    delayMicroseconds(2000);
    data_toSend[3] = analogRead(0);
    data_toSend[4] = analogRead(1);
    data_toSend[5] = analogRead(2);
    digitalWrite(mod_17k,LOW);
    delayMicroseconds(200);
    xprec1[0] = data_toSend[0];
    xprec2[0] = data_toSend[1]; 
    xprec3[0] = data_toSend[2];
    xprec4[0] = data_toSend[3];
    xprec5[0] = data_toSend[4]; 
    xprec6[0] = data_toSend[5];
    Te = (millis() - Tbeg)/1000;
    sol1 = Fkalman(xprec1,data_toSend[0],P1,Te);
    sol2 = Fkalman(xprec2,data_toSend[1],P2,Te);
    sol3 = Fkalman(xprec3,data_toSend[2],P3,Te);
    sol4 = Fkalman(xprec4,data_toSend[3],P4,Te);
    sol5 = Fkalman(xprec5,data_toSend[4],P5,Te);
    sol6 = Fkalman(xprec6,data_toSend[5],P6,Te);
    Tbeg= millis();
}


void loop()
{
    tick();
}


void tick() {
    //Previous value for filtering
    P1[0] =sol1[2]; P1[1] =sol1[3]; P1[2] =sol1[4]; P1[3] =sol1[5];
    P2[0] =sol2[2]; P2[1] =sol2[3]; P2[2] =sol2[4]; P2[3] =sol2[5];
    P3[0] =sol3[2]; P3[1] =sol3[3]; P3[2] =sol3[4]; P3[3] =sol3[5];
    P4[0] =sol4[2]; P4[1] =sol4[3]; P4[2] =sol4[4]; P4[3] =sol4[5];
    P5[0] =sol5[2]; P5[1] =sol5[3]; P5[2] =sol5[4]; P5[3] =sol5[5];
    P6[0] =sol6[2]; P6[1] =sol6[3]; P6[2] =sol6[4]; P6[3] =sol6[5];
    xprec1[0] =sol1[0]; xprec1[1] =sol1[1];
    xprec2[0] =sol2[0]; xprec2[1] =sol2[1];
    xprec3[0] =sol3[0]; xprec3[1] =sol3[1];
    xprec4[0] =sol4[0]; xprec4[1] =sol4[1];
    xprec5[0] =sol5[0]; xprec5[1] =sol5[1];
    xprec6[0] =sol6[0]; xprec6[1] =sol6[1];

    free(sol1);                                      
    free(sol2);
    free(sol3);
    free(sol4);                                    
    free(sol5);
    free(sol6);

    //Get data
    digitalWrite(mod_11k,HIGH);
    delayMicroseconds(2000);
    data_toSend[0] = analogRead(0);   //phi1 11k
    data_toSend[1] = analogRead(1); //phi2 11k
    data_toSend[2] = analogRead(2);
    digitalWrite(mod_11k,LOW);
    delayMicroseconds(200);
    digitalWrite(mod_17k,HIGH);
    delayMicroseconds(2000);
    data_toSend[3] = analogRead(0);
    data_toSend[4] = analogRead(1);
    data_toSend[5] = analogRead(2);
    digitalWrite(mod_17k,LOW);
    delayMicroseconds(200);
    Te = (millis() - Tbeg)/1000;

    //Filter
    sol1 = Fkalman(xprec1,data_toSend[0],P1,Te);          
    sol2 = Fkalman(xprec2,data_toSend[1],P2,Te);
    sol3 = Fkalman(xprec3,data_toSend[2],P3,Te);
    sol4 = Fkalman(xprec4,data_toSend[3],P4,Te);           
    sol5 = Fkalman(xprec5,data_toSend[4],P5,Te);
    sol6 = Fkalman(xprec6,data_toSend[5],P6,Te);
    Tbeg= millis();

    data_toSend[0] = sol1[0];
    data_toSend[1] = sol2[0];
    data_toSend[2] = sol3[0];
    data_toSend[3] = sol4[0];
    data_toSend[4] = sol5[0];
    data_toSend[5] = sol6[0];

    //Send data
    Codage(data_toSend,tab_1);
    HeaderSendnUInt8_RS232(tab_1,NBBYTE2SEND,Header); 
}

void Codage(int16_t *d_send, uint8_t *tab_Send)
{
    int i;
    for(i=0; i<NBDATASEND ;i++) 
    {
       d_send[i]=d_send[i]+8192;     
       tab_Send[2*i]= (uint8_t) ((d_send[i] & 127));              //lấy 7 bit đầu
       tab_Send[2*i+1]= (uint8_t) ((d_send[i] & 16256)>>7);       //lấy 7 bit sau
    }
}  

uint8_t HeaderSendnUInt8_RS232(uint8_t *ToSend, uint8_t NbToSend, uint8_t Header)
{
    Serial1.write(&Header,1);         //Header đâu phải pointer??
    return(Serial1.write((const uint8_t*)ToSend, NbToSend));
}

float *Fkalman(float xprec[2],float ymes,float P[4],float Te){ 
                     // Filtre proprement dit  
    float *sol = NULL ;
    sol = (float*) malloc (sizeof(float) * 6);
    float xpred[2];
    float Ppred[4] ;
    float Kf[2] ;
    int q = 0.05;
    int r = 100;

    //x prediction
    xpred[0] = xprec[0] + Te*xprec[1];
    xpred[1] = xprec[1]; 

    //P_pred = F.P.F'+Q
    Ppred[0] = q + P[0]+ Te*(P[2] +P[1]+Te*P[3]);
    Ppred[1] = P[1] + Te*P[3];
    Ppred[2] = P[2] + Te*P[3] ;
    Ppred[3] = q+ P[3];

    //Kf = (P_pred.C').inv(C.P_pred.C'+R)
    Kf[0]  = (Ppred[0]/(r+Ppred[0]));
    Kf[1]  = (Ppred[2]/(r+Ppred[0]));

    //X_est  
    sol[0] = xpred[0] + Kf[0]*(ymes-xpred[0]);
    sol[1] = xprec[1] + Kf[1]*(ymes-xprec[0]);

    //P_est = (I-Kf*C)*P_pred
    sol[2] = Ppred[0]*(1-Kf[0]);
    sol[3] = Ppred[1]*(1-Kf[0]);
    sol[4] = -Kf[1]*Ppred[0]+Ppred[2];
    sol[5] = -Kf[1]*Ppred[1]+Ppred[3];
    return sol;
  }

If you find any problem related to the Fkalman function or my loop function, please tell me. Thank you!!

1

It's not recommended, however you're only working with small amounts, and the same amount each time (6 floats per "sol" and 6 "sols", so 36 floats, or 144 bytes). Since it's always the same amount I would expect the same block of memory to be allocated each time.

Personally I would use local array variables in the calling function and pass them as a parameter to the function(s) that manipulate them, rather than returning a pointer to an allocated array.

float sol1[6];
Fkalman(xprec1,data_toSend[0],P1,Te,sol1);

....

void Fkalman(float xprec[2],float ymes,float P[4],float Te, float *sol){ 

etc.

However I can't see it causing an issue, so if you would clarify "doesn't work as well" we may be able to point you in a better direction.

0

As Majenko already explained it is only 144 bytes. You can use local variables as he proposes.

If you need to keep the values over multiple calls to the Fkalman function, create the space as a variable of an array for each of the calls (if the function is called a fixed number of times).

Besides this, malloc cost some time and for performance reasons it is better to do this during start up once, so the loop function is not hindered by mallocs and free.

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.