## Implementing my own EKF for height estimation

Firmware/software/electronics/mechanics
kyubot
Beginner
Posts: 4
Joined: Mon May 28, 2018 8:40 am

### Implementing my own EKF for height estimation

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.

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 = P + (P + P + (P + 0.25f*_Q_accel_dtdt) * dt) * dt;
P = P + (P + 0.5f*_Q_accel_dtdt) * dt;
P = P + (P + 0.5f*_Q_accel_dtdt) * dt;
P = P + _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 + hstate->R_altitude);
// Calculate the Kalman gain
float K = { P * Sinv, P * Sinv};
// Update the state estimate
h += K * y;
v += K * y;
// Calculate the state estimate covariance
P = P - K * P;
P = P - K * P;
P = P - (K * P);
P = P - (K * P);
}
``````
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.
Attachments
elevateFromGround.csv