Problems with accelerometer integration to get altitude

[JRD_Propulsion]

So I know that accelerometer double integration is usually a no-go for measuring position but I have seen quite a few people who have had success with the exact same sensor and the exact same purpose for me. I have attempted to land a model rocket propulsively and have gotten pretty close. However, I used a barometer(ms5607) to measure altitude and during descent, it was no longer accurate because of the high airspeed. To combat this, I thought of integrating the BMI088 twice to get altitude and since each landing test was less than 10 seconds, I thought it would be ok. In the following video, Mark from Project Horizon successfully found position using only the BMI088. In his conditions, the rocket reached 27.4 meters and in the end, was only off by less than 0.5 meters.

Link to video: https://www.youtube.com/watch?v=56R21lG72Ek&t=0s

So anyway, I knew that it is 100% possible to do what I am doing because Mark and a few others I know have successfully implemented this.

Here is my code:

/*
  Get position data by globalizing acceleration data and double intergrating it
*/
void global_position(double dt, double body_acc_x, double body_acc_y, double accel_z_cal,
                     double global_ori_z, double global_ori_y, double global_ori_x) {

  double yaw = global_ori_x * DEG_TO_RAD;
  double pitch = global_ori_y * DEG_TO_RAD;
  double roll = global_ori_z * DEG_TO_RAD;

  // Find the Z-axis acceleration in the global reference frame using rotation matrix
  //global_acc_z = (body_acc_z * cos(roll));


  // Resolve Z-axis acceleration into true vertical acceleration
  //global_acc_z = body_acc_z * cos(yaw) - 9.8;

  //global_acc_z += global_acc_z_bias;
  // During sensor calibration, set the bais so we can later remove the gravity component
  if (state == 0) {
    global_acc_z_bias = abs(accel_z_cal);
  }

  // When pad idle, keep first measurements ready but do not intergrate yet!
  if (state == 1) {
    vel_z = dt * accel_z_cal;
    pos_z = (dt * vel_z) + ((0.5 * accel_z_cal) * dt * dt);
  }

  // Ok, start intergrating, we have liftoff!
  if (state > 1) {
    vel_z += dt * accel_z_cal;
    pos_z += (dt * vel_z) + ((0.5 * accel_z_cal) * dt * dt);
  }

  // Test output
  SerialUSB.print("Global Z-axis acceleration: ");
  SerialUSB.println(pos_z);
  SerialUSB.println(body_acc_z);
}

Btw, that code is just 1 function in a much larger piece of code. And here are my accelerometer settings:

  accel.setOdr(Bmi088Accel::ODR_200HZ_BW_20HZ);
  accel.setRange(Bmi088Accel::RANGE_6G); 

So I tried a bunch of things. First, I tried to integrate just my body frame acceleration (raw data), subtract gravity (raw data + 9.8), and integrate. This didnt end up too well because although it seemed to kind of get the velocity and position as I moved the flight computer upwards, it the velocity would never decrease. It would either just keep increasing if I was moving the computer upwards in real life or just become a constant when I moved it back down. All of this makes sense except that it never returned to zero. It only returned to like 2 or 3 m/s and so that threw off the entire position estimate. Within 1 or 2 seconds, we were at like 10 or 20 meters. Then I tried a few different methods to try and get global acceleration. I tried using rotation matrices and even tried resolving for the true z-axis acceleration using trigonometry, just the body frame z-axis acceleration, and the global frame orientation (using quaternions from the gyros), but I couldnt get that to work. That method was what Mark used, I asked him. He agreed that in 2 dimensions, Z-acceleration * cos(angle from vertical) - 9.8 m/s = true acceleration but in 3 dimensions you have to do something else. After that, I had no idea what to do because apparently, not many people actually have become successful in this. So yeah, I have reached a dead-end. Some advice would be much appreciated! Thanks in advance!

Edit: Those calculations were not correct...

[Railroader]

Please cut out old things like failed attempts, all irrellevant references, and present pure facts. That will make more helpers join in. Me? Bed time since long, to tired to dig down in the pile You posted.

[hammy]

You can’t use an accelerometer for measuring distance , what you seeing are the errors in trying to do so.

On it way down ( or prob up ) once terminal velocity is obtained it’s not accelerating - so at terminal velocity it could fall ( or rise) 10 or 1000m and the accelerometer would register the same .
.

[cedarlakeinstruments]

@hammy

You can’t use an accelerometer for measuring distance

You can, but it depends on the time and accuracy needed. I worked on a project that measured braking performance by integrating acceleration to determine how much distance was required to stop a vehicle in different configurations. However, our maximum time from brake application to complete stop was, IIRC, about 10 seconds.

One of the difficulties, and OP may be running into this, is that for it to work effectively, you have to measure the acceleration along the vector that the vehicle is moving. That means that you have to compensate for any rotation that might be present.

[JRD_Propulsion]

Ok so it got better. It definitely cannot be used in this form because I have not properly removed bias and also i think there is a bug in the global z-acceleration calculation.

So this image was the old code and the blue line is position from accelerometer. I ran up and down stairs whose max height is around 5m. Below is the new graph with new code where blue is raw barometer and red is position from accelerometer:

Yes, it does look worse but that was because there was a bug since the accelerometer read -0.15m/s^2 when stationary for the z-axis global acceleration. But I think there is a similarity between the red and blue line when you look at how the derivative at each point changes.

/*
  Get position data by globalizing acceleration data and double integrating it
*/
void accel_position(double dt) {

  // Find the Z-axis acceleration in the global reference frame
  global_acc_z = body_acc_z / (cos(global_ori_y * DEG_TO_RAD) * cos(global_ori_z * DEG_TO_RAD));
  
  // Remove gravity component from global Z-axis acceleration
  global_acc_z -= global_acc_z_bias;

  // During sensor calibration, set the bias so we can later remove the gravity component
  if (state == 0) {
    global_acc_z_bias = global_acc_z;
  }

  // When pad idle, keep first measurements ready but do not integrate yet!
  if (state == 1) {
    //global_acc_z_bias = global_acc_z;
    vel_z = dt * global_acc_z;
    pos_z = (dt * vel_z) + ((0.5 * global_acc_z) * dt * dt);
  }

  // Ok, start integrating, we have liftoff!
  if (state > 1) {
    vel_z += dt * global_acc_z;
    pos_z += (dt * vel_z) + ((0.5 * global_acc_z) * dt * dt);
  }

  // Test output
  SerialUSB.print("Global Z-axis acceleration: ");
  SerialUSB.print(global_acc_z);
  SerialUSB.print("     Position: ");
  SerialUSB.println(pos_z);
}

here is the new code. I think the global z-acceleration formula I derived using trig only, while mathematically it makes sense, is somehow wrong. So any recommendations as to how I should get better global acceleration values?

[van_der_decken]

Here's a quickie chart I threw together from some ancient flight data I had laying around. Barometric altitude and accelerometer data, with the latter integrated to get speed and altitude. It worked well enough to detect things like burnout and apogee. After the chute popped the accelerometer data wasn't of any use of course. In your case where you're not just floating down but accelerating downward at 1g, I would think you've got a good chance of getting reasonable altitude data.

[JRD_Propulsion]

Hi. Ok so i got the global acceleration to work real nice. I tested it for 10 seconds and no matter the orientation, the z-acceleration is always -9.8m/s^2 or really close to it like max deviation was like +-0.1m/s^2. But i guess the integration or something is wrong. It also acts super delayed which is not cool.

code for global acceleration(using Perry's quaternion library):

void inertialAccel(double body_acc_z, double body_acc_y, double body_acc_x, double global_ori_z, double global_ori_y, double global_ori_x, double dt) 
{
  oriX += gyro_x_cal;
  oriY += gyro_y_cal;
  oriZ += gyro_z_cal;

  double oriNorm = sqrt((oriX * oriX) + (oriY * oriY) + (oriZ * oriZ));
  oriNorm = max(abs(oriNorm), 1e-12);

  orientation = Quaternion::from_axis_angle(dt * oriNorm, oriX / oriNorm, oriY / oriNorm, oriZ / oriNorm);

  measuredGravity = Quaternion(0.0, body_acc_x, body_acc_y, body_acc_z);
  worldGravity = orientation.rotate(measuredGravity);

  aX = worldGravity.b;
  aY = worldGravity.c;
  aZ = worldGravity.d;
}

My integration code is the same as before.Here are the results from another stairs test I did...

[JRD_Propulsion]

Woah, that looks really good! I hope I can get results like that! So I gotta ask, could you share how you double integrated to get position? I couldn't really get Riemann sums to work as you saw in that graph...

[van_der_decken]

I'd have to dig around and see if any notes from 25 year ago have survived to be able to give you a detailed answer. In broad strokes, it would have been summing up the area under the acceleration curve to get velocity, and doing the same for the velocity curve to get altitude. Pure one dimensional assumption stuff - but most flights were close enough to vertical for it not to matter. Every 20ms I'd have calculated the area of the rectangle up to the previous reading, and added in the triangle formed from the previous and current reading above the rectangle. Fixed point math, so limited precision - but fast. With a 4MHz processor, it had to be.

Edit: doing some quick back of the envelope calculations in Excel on the original acceleration data, I may have gone even simpler. I may have just done the sum of the area of the rectangle on the current reading times the interval, added to the previous area sum. There likely wasn't enough program space for the additional triangle calculation. So something like:

   every 20ms:
      take an acceleration reading
      multiply it by 0.02 and add it to the previous velocity to get the current velocity
      multiply the current velocity by 0.02 and add it to the previous altitude to get the current altitude

[JRD_Propulsion]

hmm, thats exactly what I did... Maybe I just have to take some real world data and apply the integration because looking at my orientation data from my stairs testing, I can see a ton of spikes, which usually are not there in my actual tvc flight data.

[van_der_decken]

if it'll help, I can give you a csv file with acceleration data and calculated velocity and altitude to try your code out on and see where/if it diverges. It won't be 100% accurate because all the data was rounded off to 1 decimal place in the original file, but the recalculated velocity and altitude values were still within +/- 0.1 when I tried the q&d method.

[JRD_Propulsion]

Sure, ill take the data, thanks! So I actually tried doing that in google sheets but i didn't know how to automate the integral calculations... Any ideas?

[van_der_decken]

Here you go:

example.zip (11.0 KB)

All I did was set up 2 extra columns topped with 0, then do the sums in the cell below, and copy and paste that those formulae the way to the end, like so:

Never tried google sheets but there's likely a similar way to accomplish the same end.

[JRD_Propulsion]

So I actually tried my TVC landing test 1 data first and got this... yeah my accelerations dont make sense at all. During ascent, the rocket had tvc and was mostly vertical. Btw, at the end, you cant trust the measurements because the rocket crashes into the ground. Anyway so then I tried your data and got incredible results, same integration method but instead of multiplying by 0.02, i multiplied by: (current time-last time)

So what accelerometer did you use? any calibration or any settings changed?

[van_der_decken]

Which accelerometer? That took some digging to unearth; discovered one last lonely one in my parts drawers. It was an Analog Devices ADXL150JQC. +/- 25/50g, single axis. Analog output. It hit end of life at least 15 years ago at a guess. I remember feeding that through a CMOS opamp into a 10bit ADC. I vaguely remember calibrating the output by standing it on either end and then laying it flat and finally storing a calibration factor and maybe an offset... but that's it. It has been almost 25 years!

[JRD_Propulsion]

Success! Yellow = barometer, red = position estimate using accelerometer only, blue = complementary filter between the two.


I did a ton of running up and down stairs testing and it works! Above are just a few examples. Usually, after about 5 seconds, the position estimates become unusable, but sometimes they can go on for longer. After measuring the actual distance, the accelerometer estimate was deadly close for the first 5 seconds! And looking back at my landing tests, I only need accurate altitude for 4-5 seconds before the retro motor fires and altitude doesn't really matter anymore. I still feel like I can maybe decrease the drift slightly if the calibration code was a little better. Although I will have to do an actual flight or landing test to really see how good this will work in practice.