kalman filter

Projects Programming
1

Hi guys.
Could you tell me how to properly add delay time for the loop, when i add delay(1000), my data is getting crazy.

#include <Wire.h>
#include <MPU6050.h>
#include <KalmanFilter.h>

MPU6050 mpu;
KalmanFilter kalmanX(0.01, 0.03, 0.3);
KalmanFilter kalmanY(0.01, 0.03, 0.3);
float accPitch = 0;
float accRoll = 0;
float kalPitch = 0;
float kalRoll = 0;

void setup()
{
Serial.begin(9600);
mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G);
mpu.calibrateGyro();
}

void loop()
{
static long lastTime=0;
long nowTime = micros();
double time = (double)(nowTime - lastTime);
time = max(time,20);
time /= 1000000;
lastTime = nowTime;

Vector acc = mpu.readNormalizeAccel();
Vector gyr = mpu.readNormalizeGyro();
accPitch = -(atan2(acc.XAxis, sqrt(acc.YAxisacc.YAxis + acc.ZAxisacc.ZAxis))*180.0)/M_PI;
accRoll = (atan2(acc.YAxis, acc.ZAxis)*180.0)/M_PI;
kalPitch = kalmanY.update(accPitch, gyr.YAxis,time);
kalRoll = kalmanX.update(accRoll, gyr.XAxis,time);
Serial.print(kalPitch);
Serial.print(":");
Serial.print(kalRoll);
Serial.print(":");
///////delay(1000);
}

2

Use the approach in the BlinkWithoutDelay example sketch.

3

Could you tell me how to properly add delay time for the loop

No. Why do you think you want to?

Doing something once per second and delaying for a second are NOT the same thing.

Look at the blink without delay example.