Arduino Boot

[thomasklaush]

After i used the Example, the Arduino didn't booted anymor. After a lon search i found the term, which is killing the arduini. KALMAN<Nstate,Nobs> K; as soon as i implement this, the Arduini is not booting anymore after the upload. PC cannot find it anymore. I have to do the double reset to send a new program. I tried it on the Arduino Nano33 IoT and the MKR 1010 (Both SMAD)

[rfetick]

It seems that the #include works properly. With the KALMAN<Nstate,Nobs> you start to use SRAM memory. Maybe you experienced a memory overflow. Do you use the official Arduino IDE to upload your program to the Arduino? Did you get any warning about high SRAM usage? Which example file did you use? And did you modify it?

[thomasklaush]

Hi, yes, the original IDE 1,8,10 & 1.8.9 (I made an update because i deleted everything to eliminate the error) There was no compiler message or warning. It was step or full example, I hadn't time to try again. For the examples no modification. In my projekt I deleted all new parts until I got it work again after commented this. I tried several times, when i uncomment it, the arduino is not booting

[rfetick]

I also use the 1.8.9, nevertheless I don't have the Nano33 IoT and the MKR1010. For information I tested it successfully on the Arduino Uno and the Nano (ATmega328P, old bootloader). Both "kalman_full" and "kalman_step" examples work fine.

Did you try to use BasicLinearAlgebra (BLA) apart? The Kalman filter library is strongly based on BLA and uses its formalism. A small arduino test could be

#include "BasicLinearAlgebra.h"
using namespace BLA;
BLA::Matrix<2,2> H;

void setup(){ }
void loop(){  }

[thomasklaush]

To find the Error i commented everything, to get the Arduino work. then I started Top down to integrate the things to find the Error.

So at least the obs Vector worked with the Matrix definition. See the code attached (Its not my code, since I cannot get there, but it was almost the same.) I try it today evening and give you a more detailed info. I will try to make a real Matrix, no Vector. Maybe the BLA is not working here.

#include "Kalman.h"
using namespace BLA;

//------------------------------------
/****  MODELIZATION PARAMETERS  ****/
//------------------------------------

#define Nstate 3 // position, speed, acceleration
#define Nobs 2   // position, acceleration

// measurement std
#define n_p 0.3
#define n_a 5.0
// model std (1/inertia)
#define m_p 0.1
#define m_s 0.1
#define m_a 0.8

BLA::Matrix<Nobs> obs; // observation vector
//KALMAN<Nstate,Nobs> K; // your Kalman filter
.
.
.

[thomasklaush]

Your code above worket, so the Matrix shouldn't be the issue.

[thomasklaush]

If I construct it manually it works...

#include <BasicLinearAlgebra.h>
using namespace BLA;
//#include <Kalman.h>

//------------------------------------
/****  MODELIZATION PARAMETERS  ****/
//------------------------------------

#define Nstate 2 // position, speed, acceleration
#define Nobs 2   // position, acceleration
#define Ncom 0   // position, acceleration

BLA::Matrix<Nobs> obs; // observation vector
//KALMAN<Nstate, Nobs> K; // your Kalman filter
//BLA::Matrix<Nstate, Nobs> K;

BLA::Matrix<Nstate,1> NULLCOMSTATE;
BLA::Matrix<Nstate,Nstate> Id; // Identity matrix
BLA::Matrix<Nstate,Nstate> F; // time evolution matrix
    BLA::Matrix<Nobs,Nstate> H; // observation matrix
    BLA::Matrix<Nstate,Ncom> B; // Command matrix (optional)
    BLA::Matrix<Nstate,Nstate> Q; // model noise covariance matrix
    BLA::Matrix<Nobs,Nobs> R; // measure noise covariance matrix
    BLA::Matrix<Nstate,Nstate> P; // (do not modify, except to init!)
    BLA::Matrix<Nstate,1> x; // state vector (do not modify, except to init!)
    BLA::Matrix<Nobs,1> y; // innovation
    BLA::Matrix<Nobs,Nobs> S;
    BLA::Matrix<Nstate,Nobs> K; // Kalman gain matrix
    int status; // 0 if Kalman filter computed correctly
    bool verbose; // true to print some information on Serial port
    bool check; // true to check obs data before applying filter, and x estimate after filter

void setup() {
  Serial.begin(9600);
  delay(3000);
  Serial.println();

  // put your setup code here, to run once:
  verbose = true;
  check = true;
  if(verbose){
    Serial.println("KALMAN:INFO: init <"+String(Nstate)+","+String(Nobs)+"> filter");
    if((Nstate<=1)||(Nobs<=1)){
      Serial.println("KALMAN:ERROR: 'Nstate' and 'Nobs' must be > 1");
    }
  }
  P.Fill(0.0);
  x.Fill(0.0);
  Id.Fill(0.0);
  for(int i=0;i<Nstate;i++){
    Id(i,i) = 1.0;
  }
  NULLCOMSTATE.Fill(0.0);

}

void loop() {
  // put your main code here, to run repeatedly:
  //Serial.println("Test");

}

void pupdate(BLA::Matrix<Nobs> obs, BLA::Matrix<Nstate> comstate){
  if(check){
    for(int i=0;i<Nobs;i++){
      if(isnan(obs(i)) || isinf(obs(i))){
        if(verbose){Serial.println("KALMAN:ERROR: observation has nan or inf values");}
        status = 1;
        return;
      }
    }
  }
  // UPDATE
  x = F*x+comstate;
  P = F*P*(~F)+Q;
  // ESTIMATION
  y = obs-H*x;
  S = H*P*(~H)+R;
  K = P*(~H)*S.Inverse(&status);
  if(!status){
    x += K*y;
    P = (Id-K*H)*P;
    if(check){
      for(int i=0;i<Nstate;i++){
        if(isnan(x(i)) || isinf(x(i))){
          if(verbose){Serial.println("KALMAN:ERROR: estimated vector has nan or inf values");}
          status = 1;
          return;
        }
      }
    }
  }else{
    if(verbose){Serial.println("KALMAN:ERROR: could not invert S matrix. Try to reset P matrix.");}
    P.Fill(0.0); // try to reset P. Better strategy?
    K.Fill(0.0);
  }
};

void update(BLA::Matrix<Nobs> obs, BLA::Matrix<Ncom> com){
  if(check){
    for(int i=0;i<Ncom;i++){
      if(isnan(com(i)) || isinf(com(i))){
        if(verbose){Serial.println("KALMAN:ERROR: command has nan or inf values");}
        status = 1;
        return;
      }
    }
  }  
  pupdate(obs,B*com);
};

[rfetick]

There is an issue with the import, what is strange is that the #include <Kalman.h> didn't raise any error and the compiler did its job without complaining.

For temporary solution you can also try to move the Kalman files in the same folder as your current project.

How did you install the library? Did you download it from the IDE library manager or manual installation from Github?

I also see that your Q and R matrices don't use the Symmetric template as I implemented in the most recent version. Similarly Id should use its dedicated template. I think your Kalman version is not the most recent one. However the anterior versions should be self consistent and not produce the kind of error you encountered...

EDIT: I just updated the release version. So if you downloaded Kalman from the IDE library manager, you should find the new version (this might not solve your problem).

[thomasklaush]

I programmed it ithout a class, the funtions as well. 4x4 Matrix is working well and fast. I even incuded the Symmetric part and so on. I tried everythin, included the files in the program and used " instead of < and so on. Nothing worked. It seems, as the SAMD Chip doesn't work with some sttff from the class.

[rfetick]

You can try to build again the Kalman class step by step in your ino file and see when the issue appears.

Otherwise you can just give up the class as you already did. You may move all the Kalman definitions and functions in a separated header file so they don't pollute your main ino file.