private void filtrKalmana()
{
if ((ADXL345.koniecKalibracji == true) && (MPU6050_Akcel.koniecKalibracji == true) &&
(L3G4200D.koniecKalibracji == true) && (MPU6050_Giro.koniecKalibracji == true))
{
_kalman.roll1 = _kalman.update(_orientacjaKatowa.rollADXL345, L3G4200D.omegaKal[0], 1 / czestotliwosc);
_kalman.pitch1 = _kalman.update(_orientacjaKatowa.pitchADXL345, L3G4200D.omegaKal[1], 1 / czestotliwosc);
_kalman.roll2 = _kalman.update(_orientacjaKatowa.rollMPU6050, MPU6050_Giro.omegaKal[0], 1 / czestotliwosc);
_kalman.pitch2 = _kalman.update(_orientacjaKatowa.pitchMPU6050, MPU6050_Giro.omegaKal[1], 1 / czestotliwosc);
}
wypiszKalmana();
}
public void updateAllClasses()
{
//Update filter for pedometer
fbwX.Update(Input.acceleration.x);
// Update using the filter for acceleration data related to gravity finding in X, Y, Z
findGX.Update(Input.acceleration.x);
findGY.Update(Input.acceleration.y);
findGZ.Update(Input.acceleration.z);
//Update pedometer
pedometer.update(fbwX.Value);
//update our angle of rotation from the magnetometer readings
magnetoAxisRotationAngles.update_compass(Input.compass.rawVector.x, Input.compass.rawVector.y, Input.compass.rawVector.z);
// Update the gyro for old method just integrating gyro data
gyroIntegrator.update_orientation(Input.gyro.rotationRate.x, Input.gyro.rotationRate.y, Input.gyro.rotationRate.z);
// Initialise the gyro with compass values if need be
if (!gyroIntegrator.isInitialised)
{
gyroIntegrator.init_gyro_values(magnetoAxisRotationAngles.aboutX, magnetoAxisRotationAngles.aboutY, magnetoAxisRotationAngles.aboutZ);
gyroIntegrator.isInitialised = true;
// Initialise the Kalman values
kalmanX.init_kalman_values(magnetoAxisRotationAngles.aboutX);
kalmanY.init_kalman_values(magnetoAxisRotationAngles.aboutY);
kalmanZ.init_kalman_values(magnetoAxisRotationAngles.aboutZ);
kalmanX.isInitialised = true;
kalmanY.isInitialised = true;
kalmanZ.isInitialised = true;
}
//update kalman filter in x, y, z using raw gyro and
kalmanX.update(Input.gyro.rotationRate.x, magnetoAxisRotationAngles.aboutX);
kalmanY.update(Input.gyro.rotationRate.y, magnetoAxisRotationAngles.aboutY);
kalmanZ.update(Input.gyro.rotationRate.z, magnetoAxisRotationAngles.aboutZ);
// old method of Kalman using integrated gyro term to compare
// kalmanZ.update(gyroIntegrator.orientationZ, magnetoAxisRotationAngles.aboutZ);
//update heading
// heading.updateHeading(magnetoAxisRotationAngles.aboutZ, magnetoAxisRotationAngles.aboutX, findGX.outputHistory[0], findGY.outputHistory[0], findGZ.outputHistory[0]);
heading.updateHeading(kalmanZ.xhat, kalmanX.xhat, findGX.outputHistory[0], findGY.outputHistory[0], findGZ.outputHistory[0]);
filteredHeading.Update((float)heading.current_heading);
}