Using global velocity in body frame

Hi,

I want to use the lateral (vy) velocity of my drone in body reference frame for outdoor application.
So basically flight speed left/right in m/s.

I see that I can get global velocities (North, East, Down) from:
const Vector3f &vel = inertial_nav.get_velocity()

But I need the local/body velocities, preferably fused with GPS, for a lateral speed prediction step used to filter sonar measurements for bad measurements.

Any ideas?

Cheers,

Jon

Hi,

You can get body frame velocity by multiplication with yaw angle:

float roll_vel =  vel.y * ahrs.cos_yaw() - vel.x * ahrs.sin_yaw(); // body roll vel
float pitch_vel = vel.y * ahrs.sin_yaw() + vel.x * ahrs.cos_yaw(); // body pitch vel

Your idea of using velocity to filter sonar is very interesting to me, because I am currently using sonar and the reading are just too noisy to be useful. However, when you encounter sudden change in the profile (for eg, the copter pass through a corridor that has a open door at the side, there will be a sudden jump in reading), how do you make sure such scenario are taken into account? Would love to hear more from you about this. Thanks.

MQ

Hi,

Thanks for the fast reply and my apologies for my late reply.
I tried your code and it works very well. Thank you!

How my sonar filter works:
I assume I only want to filter out bad peak values which 95% of the time only last one measurement and at most two. Therefore I look at the jumps in measurements and assume that if they last longer than 2 measurements they are a valid change of the environment. The disadvantage of this approach is that it takes 2-3 cycles for valid changes to come through, but when firing at +5Hz, this is time delay is acceptable for slow flight.

Every time it gets a new measurement, it calculates the standard deviation of the last 5 elements. Then it compares the last 2 or 3 standard deviations and find their maximum. If the maximum is lower than a certain threshold (I chose 30cm) we assume the value is valid and use the new measurement. If not, then that means there is a (single or double) jump present and this should be filtered out. It then uses the velocity of the UAV in the direction of the sonar to predict the new measurement assuming the virtual wall the sonar senses is standing still. This way the guessed measurement value is usually close to the real value.

A real Kalman Filter might perform better, but this is acceptable for my application.

Cheers,

Jon

1 Like