Implementing my own EKF for height estimation
Posted: Mon Jun 25, 2018 7:54 am
I am trying to implement my own kalman filter to get more accurate height estimation fusing Acc, barometer and sonar data.
Something similar to this https://youtu.be/JoWU8qoAk84
https://youtu.be/JoWU8qoAk84
I barely could implement for fusing accelerometer about z axis and barometer data here.
I still have difficulty with fusing sonar data to this. Is there any references I can count on?
Any advice or help would greatly appreciated.
For your reference, I attached sensor data I gathered from cfclient while moving drone with hand.
Something similar to this https://youtu.be/JoWU8qoAk84
https://youtu.be/JoWU8qoAk84
I barely could implement for fusing accelerometer about z axis and barometer data here.
Code: Select all
static void positionUpdateVelocityInternal(float accWZ, float dt, struct selfState_h* hstate){
float _dtdt = dt * dt;
h = h + v * dt + 0.5f * accWZ * _dtdt;
v = v + accWZ * dt;// * G;
// Calculate the state estimate covariance
float _Q_accel_dtdt = hstate->Q_Accel * _dtdt;
P[0][0] = P[0][0] + (P[1][0] + P[0][1] + (P[1][1] + 0.25f*_Q_accel_dtdt) * dt) * dt;
P[0][1] = P[0][1] + (P[1][1] + 0.5f*_Q_accel_dtdt) * dt;
P[1][0] = P[1][0] + (P[1][1] + 0.5f*_Q_accel_dtdt) * dt;
P[1][1] = P[1][1] + _Q_accel_dtdt;
}
static void positionEstimateKalmanHeightInternal(state_t* estimate, const sensorData_t* sensorData, float dt, uint32_t tick, struct selfState_h* hstate) {
// Calculate innovation
float y = sensorData->baro.asl - h;//hstate->estimatedZ;
// Calculate the inverse of the innovation covariance
float Sinv = 1.0f / (P[0][0] + hstate->R_altitude);
// Calculate the Kalman gain
float K[2] = { P[0][0] * Sinv, P[1][0] * Sinv};
// Update the state estimate
h += K[0] * y;
v += K[1] * y;
// Calculate the state estimate covariance
P[0][0] = P[0][0] - K[0] * P[0][0];
P[0][1] = P[0][1] - K[0] * P[0][1];
P[1][0] = P[1][0] - (K[1] * P[0][0]);
P[1][1] = P[1][1] - (K[1] * P[0][1]);
}
Any advice or help would greatly appreciated.
For your reference, I attached sensor data I gathered from cfclient while moving drone with hand.