I use the Simple Kalman Filter library GitHub - denyssene/SimpleKalmanFilter: A basic implementation of Kalman Filter for single variable models., which may be of interest to you.
An example of use:
void TrackSun( void * pvParameters )
{
int Altitude = 1500;
int Azimuth = 1500;
int maxAltitudeRange = 2144;
int minAltitudeRange = 900;
int maxAzimuthRange = 2144;
int minAzimuthRange = 900;
SimpleKalmanFilter kfAltitude0( 10.0, 10.0, .01 ); // kalman filter Altitude 0
SimpleKalmanFilter kfAltitude1( 10.0, 10.0, .01 ); // kalman filter Altitude 1
SimpleKalmanFilter kfAzimuth0( 5.0, 10.0, .01 ); // kalman filter Azimuth 0
SimpleKalmanFilter kfAzimuth1( 5.0, 10.0, .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;
int64_t AzimuthEndTime = esp_timer_get_time();
int64_t AzimuthStartTime = esp_timer_get_time(); //gets time in uSeconds like Arduino Micros,
int64_t AltitudeEndTime = esp_timer_get_time();
int64_t AltitudeStartTime = esp_timer_get_time();
while (1)
{
//Altitude
AltitudeEndTime = esp_timer_get_time() - AltitudeStartTime; // produce elasped time for the simpleKalmanFilter
kfAltitude0.setProcessNoise( (float)AltitudeEndTime / 1000000.0f ); //convert time of process to uS, update SimpleKalmanFilter q
kfAltitude1.setProcessNoise( (float)AltitudeEndTime / 1000000.0f ); //convert time of process to uS, update SimpleKalmanFilter q
filteredAltitude_0 = kfAltitude0.updateEstimate( (float)adc1_get_raw(ADC1_CHANNEL_3) );
filteredAltitude_1 = kfAltitude1.updateEstimate( (float)adc1_get_raw(ADC1_CHANNEL_0) );
if ( filteredAltitude_0 > filteredAltitude_1 )
{
Altitude -= 1;
if ( Altitude < minAltitudeRange )
{
Altitude = 1500;
}
fMoveAltitude( Altitude );
vTaskDelay( 12 );
AltitudeStartTime = esp_timer_get_time();
}
if ( filteredAltitude_0 < filteredAltitude_1 )
{
Altitude += 1;
if ( Altitude >= maxAltitudeRange )
{
Altitude = 1500;
}
fMoveAltitude( Altitude );
vTaskDelay( 12 );
AltitudeStartTime = esp_timer_get_time();
}
//// AZIMUTH
AzimuthEndTime = esp_timer_get_time() - AzimuthStartTime; // produce elasped time for the simpleKalmanFilter
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_5) );
filteredAzimuth_1 = kfAzimuth1.updateEstimate( (float)adc1_get_raw(ADC1_CHANNEL_6) );
if ( filteredAzimuth_0 > filteredAzimuth_1 )
{
Azimuth += 1;
if ( Azimuth >= maxAzimuthRange )
{
Azimuth = 900;
}
fMoveAzimuth( Azimuth );
vTaskDelay( 12 );
AzimuthStartTime = esp_timer_get_time();
}
if ( filteredAzimuth_0 < filteredAzimuth_1 )
{
Azimuth -= 1;
if ( (Azimuth >= maxAzimuthRange) || (Azimuth <= minAzimuthRange) )
{
Azimuth = 900;
}
fMoveAzimuth( Azimuth );
vTaskDelay( 12 );
AzimuthStartTime = esp_timer_get_time();
}
// Serial.println();
vTaskDelay( 30 );
//
} // while(1)
} //void TrackSun()
Instead of using a static time, I use a dynamic time, which works much better.