Implementing my own EKF for height estimation

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

Implementing my own EKF for height estimation

Post by kyubot » 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.

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]);
}
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
(62.23 KiB) Downloaded 14 times
stable.csv
(20.17 KiB) Downloaded 14 times

Post Reply