Hello,
I am using a LSM6DSOX Accelerometer Gyro in a Robotic project, but only using at this the Gyro Yaw/Z axis for robot rotation monitoring. I have a solution for static/stationary drift under control by implementing simple noise filter. Problem I am having (I think) is when the robot is in motion the vibrations from its motion pass thru my filter and are presented as Gyro Drift! So bit of looking about and I am wondering if a Kalman filter wound work?
Also if you have a Kalman filter Gyro/Accelerometer Arduino Sketch to share that would be a great help please.
Many thanks in advance IMK
not with gyro/accelemeter but a task using 4 Kalman filters.
void TrackSun( void * pvParameters )
{
log_i( "Startup TrackSun" );
int64_t EndTime = esp_timer_get_time();
int64_t StartTime = esp_timer_get_time(); //gets time in uSeconds like Arduino Micros
int Altitude = 1500;
int Azimuth = 900;
int maxAltitudeRange = 2144;
int minAltitudeRange = 900;
int maxAzimuthRange = 2144;
int minAzimuthRange = 900;
float AltitudeThreashold = 10.0f;
float AzimuthThreashold = 10.0f;
float kalmanThreshold = 80.0f;
SimpleKalmanFilter kfAltitude0( AltitudeThreshold, kalmanThreshold, .01 ); // kalman filter Altitude 0
SimpleKalmanFilter kfAltitude1( AltitudeThreshold, kalmanThreshold, .01 ); // kalman filter Altitude 1
SimpleKalmanFilter kfAzimuth0( AzimuthThreshold, kalmanThreshold, .01 ); // kalman filter Azimuth 0
SimpleKalmanFilter kfAzimuth1( AzimuthThreshold, kalmanThreshold, .01 ); // kalman filter Azimuth 1
float filteredAltitude_0 = 0.0f;
float filteredAltitude_1 = 0.0f;
float filteredAzimuth_0 = 0.0f;
float filteredAzimuth_1 = 0.0f;
uint64_t AzimuthEndTime = esp_timer_get_time();
uint64_t AzimuthStartTime = esp_timer_get_time(); //gets time in uSeconds like Arduino Micros,but rolls over after 207 years
uint64_t AltitudeEndTime = esp_timer_get_time();
uint64_t AltitudeStartTime = esp_timer_get_time(); //gets time in uSeconds like Arduino Micros,
int StartupCount = 0;
int TorqueAmmount = 5;
while (1)
{
if ( EnableTracking )
{
//Altitude
AltitudeEndTime = esp_timer_get_time() - AltitudeStartTime; // produce elapsed time for the simpleKalmanFilter
kfAltitude0.setProcessNoise( float(AltitudeEndTime) / 1000000.0f );
kfAltitude1.setProcessNoise( float(AltitudeEndTime) / 1000000.0f );
// Serial.println( AltitudeEndTime,6 );
filteredAltitude_0 = kfAltitude0.updateEstimate( (float)adc1_get_raw(ADC1_CHANNEL_6) );
filteredAltitude_1 = kfAltitude1.updateEstimate( (float)adc1_get_raw(ADC1_CHANNEL_5) );
// Serial.print( filteredAltitude_0 );
// Serial.print( ", " );
// Serial.print( filteredAltitude_1 );
// Serial.print( ", " );
// Serial.println();
if ( (filteredAltitude_0 > filteredAltitude_1) && (abs(filteredAltitude_0 - filteredAltitude_1) > AltitudeThreshold))
{
Altitude -= TorqueAmmount;
// log_i( "> Alt %d", Altitude );
if ( Altitude < minAltitudeRange )
{
Altitude = 900;
}
fMoveAltitude( Altitude );
AltitudeStartTime = esp_timer_get_time();
}
if ( (filteredAltitude_0 < filteredAltitude_1) && (abs(filteredAltitude_0 - filteredAltitude_1) > AltitudeThreshold) )
{
Altitude += TorqueAmmount;
if ( Altitude >= maxAltitudeRange )
{
Altitude = 900;
}
fMoveAltitude( Altitude );
// log_i( "< Alt %d", Altitude );
AltitudeStartTime = esp_timer_get_time();
}
//// AZIMUTH
AzimuthEndTime = esp_timer_get_time() - AzimuthStartTime; // produce elasped time for the simpleKalmanFilter
// Serial.print( (float)adc1_get_raw(ADC1_CHANNEL_3) );
// Serial.print( "," );
// Serial.print( (float)adc1_get_raw(ADC1_CHANNEL_0) );
// Serial.print( "," );
kfAzimuth0.setProcessNoise( float(AzimuthEndTime) / 1000000.0f ); //convert time of process to uS, update SimpleKalmanFilter q
kfAzimuth1.setProcessNoise( float(AzimuthEndTime) / 1000000.0f ); //convert time of process to uS, update SimpleKalmanFilter q
filteredAzimuth_0 = kfAzimuth0.updateEstimate( (float)adc1_get_raw(ADC1_CHANNEL_3) );
filteredAzimuth_1 = kfAzimuth1.updateEstimate( (float)adc1_get_raw(ADC1_CHANNEL_0) );
// Serial.print( filteredAzimuth_0 );
// Serial.print( ", " );
// Serial.print( filteredAzimuth_1 );
// Serial.println();
if ( (filteredAzimuth_0 > filteredAzimuth_1) && (abs(filteredAzimuth_0 - filteredAzimuth_1)) > AzimuthThreashold )
{
Azimuth += TorqueAmmount;
if ( Azimuth >= maxAzimuthRange )
{
Azimuth = 900;
}
// log_i( "> Az %d", Azimuth );
fMoveAzimuth( Azimuth );
AzimuthStartTime = esp_timer_get_time();
}
// //
if ( (filteredAzimuth_0 < filteredAzimuth_1) && (abs(filteredAzimuth_1 - filteredAzimuth_0)) > AzimuthThreashold )
{
// log_i( "< Az %d", Azimuth );
Azimuth -= TorqueAmmount; // make new position to torque to
if ( (Azimuth >= maxAzimuthRange) || (Azimuth <= minAzimuthRange) )
{
Azimuth = 900;
}
fMoveAzimuth( Azimuth );
AzimuthStartTime = esp_timer_get_time();
} //azmiuth end
if ( StartupCount < 500 )
{
++StartupCount;
} else {
TorqueAmmount = 1;
// esp_sleep_enable_timer_wakeup( (60000000 / 30) ); // set timer to wake up
// esp_sleep_enable_timer_wakeup( (60000000 / 20) ); // set timer to wake up
// esp_light_sleep_start();
}
}
vTaskDelay( 65 );
} // while(1)
} //void TrackSun()
Yes, a Kalman filter is a good choice for filtering out noise and accurately estimating the robot's orientation, especially when dealing with motion-induced vibrations.
Here's an example of a Kalman filter implementation for a 6-axis IMU (accelerometer + gyro) on an Arduino board. You will need to modify it to only use the Gyro Yaw/Z axis data:
#include <Wire.h>
#include <Kalman.h>
Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
float gyroXangle, gyroYangle; // Angle calculated using the gyro only
float kalmanXangle, kalmanYangle; // Angle calculated using a Kalman filter
float kalmanXrate, kalmanYrate; // Unbiased rate calculated
Is fixed by calibrating the gyro. No filter can remove systematic errors.
This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.