# State estimation using accelerometer

### Help Support The Rocketry Forum:

This site may earn a commission from merchant affiliate links, including eBay, Amazon, and others.

#### diyaerospace

##### Well-Known Member
Hi,
I have been working on a custom altimeter that I plan to use to calculate altitude, velocity, position, etc. I have tested it a couple of times and the altitude calculation looks good. The velocity measurements did seem relatively accurate as well but the main problem is drift. When the rocket is straight up there is little to no drift but when pitched over 90 degrees the calculation drifts 500 meters a second every minute. This is not good enough and I did some research and it seems that quaternions can fix the drift. However I am having trouble finding a good library to use. I was wondering if anyone could help me out with this!
Ideally the quaternions could correct with the movements of the rocket and keep the z axis always pointed upward.

I am using a MPU 6050 but plan to switch to the LSM6DSO32 once they are restocked!

I use the Arduino IDE to code all of this!

Any suggestions would be appreciated, I am also only in 7th grade so most math will likely be beyond me.

Your main problem is that the things you are interested in are not "observable" using an accel. Or an accel and gyro. Drift will always be a problem. Less of a problem with more expensive sensors of course.

Your second problem is that all of the things you are interested in, state estimation, quaternions, etc. require college level math to really understand. Heck, quaternions make my brain hurt and I have a MSEE.

I also use an MPU6050 plus Magnetometer and Baro sensors.

The MPU6050 MUST be calibrated before using the data.
The Accelerometer has offsets, Laid level and one axis should read 9.8m/s/s (+ or -) and the other Zero. I find they do not. So get the 6 measurements and calculate the offset. Save these and subtract from the measurement of the flight. Excel or other spreadsheet is good for this.

Gyro also has offset errors. With the MPU6050 NOT moving the gyro readings should be ZERO. These also drift over time and temperature. It is good to save many gryo reading from just before the flight (assuming the rocket is NOT Moving so use a good solid launch pad and rail). Then subtract the gyro offset from the flight data.

Quaternions also make my brain hurt and haven't been successful using them.
There is the AHRS Python library for sensor fusion. Adafruit also has some Arduino libraries.

Here is a very good thread on using accelerometer.
https://www.rocketryforum.com/threa...ion-of-single-axis-accelerometer-data.170152/
Do read the paper he posted.
Great explanation on what an Accelerometer will measure in a rocket.

Don't forget temperature effects on gyro and accelerometer drift. Ardupilot firmware has a nice calibration process where you put the hardware in the fridge and cool it down, then quickly place it in an oven set to about 70degC and turn it on. The temperature and drift rates are measured as the hardware warms up. These measurements are then used as corrections during normal use. Quite an elegant solution.

The drift while the rocket is straight up on the pad is minimal, the problem is when the rocket(or the sensor) pitches over the drift increases to the point where the velocity calculation is no longer usable.

Here is some data from the serial plotter.
For the first half of the data, the sensor is straight up. The second half shows the sensor pitched 90 degrees.

This is how I calculate velocity
[
velocityoldx = velocitynewx;
velocityoldy = velocitynewy;
velocityold = velocitynew;
velocityy =(velocity_changey);
velocityx =(velocity_changex);
velocitynew = velocity_change;

sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);

velocity_change = ((previous_time/1000) - (Time/1000)) * (a.acceleration.z);
velocitynew = (((velocity_change)) + velocityold);

velocity_changey = ((previous_time/1000)-(Time/1000)) * (a.acceleration.y );
velocitynewy = (velocityoldy + (velocity_changey));

velocity_changex = ((previous_time/1000) - (Time/1000)) * (a.acceleration.x + 9.81);
velocitynewx = (velocityoldx + (velocity_changex)/100);

]

#### Attachments

• Screen Shot 2022-03-17 at 6.02.56 PM.png
11.1 KB · Views: 6
I think your problem is you are not subtracting 1g from your accelerometer vertical axis reading after launch.....

I think I am in this line (I add 10 meters a second squared because the sensor was positioned upside down on this launch)
velocity_changex = ((previous_time/1000) - (Time/1000)) * (a.acceleration.x + 9.81);

The drift makes sense because of how velocity is integrated.
velocityoldx = velocitynewx;
velocityoldy = velocitynewy;
velocityold = velocitynew;
velocityy =(velocity_changey);
velocityx =(velocity_changex);
velocitynew = velocity_change;

This calculation works very well(I compared the readings from this to the reading on the jolly logic altimeter 2 and they were pretty close)
The problem is when the rocket pitches over the velocity increase because of how integration is done. I am looking for ways to still get accurate velocity data but eliminate the drift.
Any suggestions will be welcome!

I think I am in this line (I add 10 meters a second squared because the sensor was positioned upside down on this launch)
velocity_changex = ((previous_time/1000) - (Time/1000)) * (a.acceleration.x + 9.81);

The drift makes sense because of how velocity is integrated.
velocityoldx = velocitynewx;
velocityoldy = velocitynewy;
velocityold = velocitynew;
velocityy =(velocity_changey);
velocityx =(velocity_changex);
velocitynew = velocity_change;

This calculation works very well(I compared the readings from this to the reading on the jolly logic altimeter 2 and they were pretty close)
The problem is when the rocket pitches over the velocity increase because of how integration is done. I am looking for ways to still get accurate velocity data but eliminate the drift.
Any suggestions will be welcome!
No that is not the reason. When the rocket is near apogee and going relatively slow your accelerometer will read close to 0 in all axes because the acceleration in a ballistic trajectory is zero by definition. You need to subtract 1g from the z-component (if z is up) to convert the rocket frame acceleration (which the accelerometer measures) to the earth frame acceleration.

You need to subtract 1g from the z-component (if z is up) to convert the rocket frame acceleration (which the accelerometer measures) to the earth frame acceleration.

Love it!

Yes, -1g.

Also, the other two Accel axis tell you nearly nothing about the flight.

The Accel's vertical axis (inline with rocket) measurements should be:
1) pre-launch = 1g (9.8m/s/s) Apply math to make this positive (multiply by -1 if measurement is negative).
Note: The rocket's initial launch angle can be calculated from the three Accel axis but ONLY with measurements while the rocket is sitting on the pad.

2) Boosting (motor burning) = should look like the motor's thrust curve and positive.

3) Coasting (from motor burn-out to apogee) = goes sharply negative at motor burn-out and slowly goes to Zero at apogee. This Negative acceleration is the rocket's DRAG. This part of the curve can be used to calculate the rocket's Drag coefficient. Handy to correct a design simulation.

The Vertical Accel's measurement will look like the Vertical Acc in any rocket simulation (Open Rocket). Compare your acc measurement to the sim as a Sanity check)

To obtain nose over angles the accel can not be used (read the paper I posted link to to understand why). This requires integrating the Gyro's Rates.

Hello,

Yes, the flight data shows what you mentioned above, my MPU 6050 has some gyros that I haven't used for pitch/yaw angles yet. On number one you mentioned having 1g on the z-axis, my accelerometers show similar values but they are not perfect because the rocket is not straight up. I did some research and I think a rotation matrix can help by rotating the vectors so the z-axis is in line with the 1g. I am a little confused on how to implement this though. Can anyone give me some guidance?
Thanks

No that is not the reason. When the rocket is near apogee and going relatively slow your accelerometer will read close to 0 in all axes because the acceleration in a ballistic trajectory is zero by definition. You need to subtract 1g from the z-component (if z is up) to convert the rocket frame acceleration (which the accelerometer measures) to the earth frame acceleration.
I don't understand what you mean here. Yes, I know that when the rocket slows down the acceleration is zero, but on my previous flight, I constantly subtracted the force of gravity during flight. Is this what you mean?

I don't understand what you mean here. Yes, I know that when the rocket slows down the acceleration is zero, but on my previous flight, I constantly subtracted the force of gravity during flight. Is this what you mean?

On rotation. By the time your rocket is pitching over the accelerometer is reading near zero. A rotation transform of {0,0,0} will also be {0,0,0} at any and all rotation angles during the pitch over. Are you launching vertical?

On rotation. By the time your rocket is pitching over the accelerometer is reading near zero. A rotation transform of {0,0,0} will also be {0,0,0} at any and all rotation angles during the pitch over. Are you launching vertical?
Sure, In the text file there is accelerometer data on the far right, estimated velocity data in the middle column, and altitude data on the far left.

I am launching relatively vertical, only a few degrees off.

In the altitude data, you can see a pretty big spike. This is because of the ejection charge unintentionally getting into the avionics bay. I wrote a filter to prevent bad measurements like this that I will implement in the next flight.

#### Attachments

• data.march6.flight1.format.txt
3.1 KB · Views: 7
• march6.flight1.plot.pdf
89.4 KB · Views: 5
Sure, In the text file there is accelerometer data on the far right, estimated velocity data in the middle column, and altitude data on the far left.

I am launching relatively vertical, only a few degrees off.

In the altitude data, you can see a pretty big spike. This is because of the ejection charge unintentionally getting into the avionics bay. I wrote a filter to prevent bad measurements like this that I will implement in the next flight.
Great, give me some time to take a look at your data, maybe by tomorrow and I should be able to help.

Sure, In the text file there is accelerometer data on the far right, estimated velocity data in the middle column, and altitude data on the far left.

I am launching relatively vertical, only a few degrees off.

In the altitude data, you can see a pretty big spike. This is because of the ejection charge unintentionally getting into the avionics bay. I wrote a filter to prevent bad measurements like this that I will implement in the next flight.
In the text file is the data just one (vertical( axis or does it include all 3 (x,y,z)?

Your problem is it appears your ejection charge fired when the rocket still had significant vertical velocity. After that point your accelerometer is measuring chaotic motion and its well beyond your skill (much less graduate students) to integrate that motion. Data attached. Next flight make sure you ejection charge fires after apogee (ideally) and then check your results.

#### Attachments

• trf-data.csv
1.1 KB · Views: 5
Your problem is it appears your ejection charge fired when the rocket still had significant vertical velocity. After that point your accelerometer is measuring chaotic motion and its well beyond your skill (much less graduate students) to integrate that motion. Data attached. Next flight make sure you ejection charge fires after apogee (ideally) and then check your results.
Is this why my velocity data was drifting?

I flew twice that day with a similar setup. On the second flight, the rocket pitched over after leaving the pad due to the fins not having enough airflow over them to stabilize the rocket. Because of this, the rocket was traveling parallel to the ground for most of the time and the reduced flight time meant that the ejection charge didn't go off until the rocket already hit the ground and destroyed all the electronics .
I still got data back though.

In the text file there is accelerometer data on the far right, estimated velocity data in the middle column, and altitude data on the far left.

#### Attachments

• march6flight.plot.pdf
100.5 KB · Views: 6
• data.march6.flight2.format.txt
1.3 KB · Views: 4
In the text file is the data just one (vertical( axis or does it include all 3 (x,y,z)?
I only plotted the data on the vertical axis, but I can send you the others if you want.

Is this why my velocity data was drifting?

I flew twice that day with a similar setup. On the second flight, the rocket pitched over after leaving the pad due to the fins not having enough airflow over them to stabilize the rocket. Because of this, the rocket was traveling parallel to the ground for most of the time and the reduced flight time meant that the ejection charge didn't go off until the rocket already hit the ground and destroyed all the electronics .
I still got data back though.

In the text file there is accelerometer data on the far right, estimated velocity data in the middle column, and altitude data on the far left.
Your velocity data is fine (right up to when the ejection charge fired). After that you are generating random numbers.

Even if you were to program the math to integrate the motion after ejection charge the Arduino does not have the processing power to integrate it. You would need around 1000hz sample rate and tons of floating point calculations. The Arduino is not going to be able to do it. Before the ejection charge fires no problem and your existing code is close. And yes you need vertical flights to make it easier.

Hello,

Yes, the flight data shows what you mentioned above, my MPU 6050 has some gyros that I haven't used for pitch/yaw angles yet. On number one you mentioned having 1g on the z-axis, my accelerometers show similar values but they are not perfect because the rocket is not straight up. I did some research and I think a rotation matrix can help by rotating the vectors so the z-axis is in line with the 1g. I am a little confused on how to implement this though. Can anyone give me some guidance?
Thanks

What you are seeing, not exactly 1g (9.80m/s/s) is the OFFSET nad GAIN ERRORs of the accelerometer readings. You MUST correct (Calibrate) for these errors.

The MPU6050 I use has offset errors of:
x = 0.9335
y = -0.15158
z = -1.48499
After getting a reading I subtract these ERRORs from the reading THEN do the calculations.
I got these offset error values by setting the Accelerometer of a level surface and took many reading. Then average the readings.
Turn the accelerometer over so the axis that was up is now down. Take reading and average.
Then do on edge up & down then the other edge up & down.
You now have 6 averaged readings.

The math for only x-axis to obtain the offset error is:
Say Xup = 9.76 & Xdown = -9.94
The offset error = (Xup + Xdown)/2 = (9.76 -9.94)/2 = -0.09
Subtract this error from the readings.

There can also be a GAIN error.
calculation is:
[(Xup - Xdown)/2] / 9.8 = [(9.76- (-)9.94)/2] /9.8 = 1.005
Divide the offset corrected reading by this gain error.

You have some data from two flights so do the calibration measurements and apply to your data. Then use this to do calc Vel & Alt.

Do note that Altitude assumes that the rocket went straight up, ALL acceleration (and Velocity) produces altitude. If the rocket goes off at an angle the altitude calculations will be TOO HIGH.
And as John has mentioned, data is good until the ejection charge goes off. After that the rockets motion is too eratic to be able to calculate. A Baro based Altitude is only sensor usable.

Last edited:
Do note that Altitude assumes that the rocket went straight up, ALL acceleration (and Velocity) produces altitude. If the rocket goes off at an angle that altitude calculations will be TOO HIGH.
But not as much as you think....

Thanks for the table.

I might try to filter out the acceleration that occurs during the parachute ejection for the next flight.
I also plan to turn this into a PCB to save space.
The IMU that I plan to use is the LSM6DSO32, because it contains an accelerometer and gyroscope. However, the 32g limit is not ideal in some flight profiles. I was thinking of using a higher g accelerometer and gyroscope separately but wouldn't this be a problem because each sensor is in a different place on the board so forces the experienced will not be equal?

No problem with two separate sensors. This is a 'rigid body' so all parts experience to same movements.

I might try to filter out the acceleration that occurs during the parachute ejection for the next flight.
I also plan to turn this into a PCB to save space.
The IMU that I plan to use is the LSM6DSO32, because it contains an accelerometer and gyroscope. However, the 32g limit is not ideal in some flight profiles. I was thinking of using a higher g accelerometer and gyroscope separately but wouldn't this be a problem because each sensor is in a different place on the board so forces the experienced will not be equal?
You do understand you have NO HOPE to determine inertial velocity or position after the separation event right?

diyaerospace

Have you done sims (Open Rocket) to get expected acceleration in G's? Unless you are going to fly some of the high power stuff a 16g acc is enough. This is a 16:1 thrust to weight ratio.
I went through this and decided the 16g MPU6050 would work fine. I didn't expect more than about 8gs of acceleration.
An issue with a higher g accelerometer is loss of resolution.

You have done quite well so far. Just a bit more study and tweaks to the math are needed.

The NAR has all their NARAM reports online and many are very good reads.
I am re-reading NARAM-58 by Bryan Sparkman now and he has very good info on calibration and calculations.

Which processor are you using? There are quite a few Arduino compatible boards that run fast. This helps getting the math done.
What sampling rate of the sensor are you using?
I run my 10DOF at 50sps (I2C at 400kHz). This limit is due to reading all sensors and writing data to FLASH memory. If less data then I could sample faster which is important for the Gyro.

I recommend adding a Baro altimeter. This then give you altitude from a different source (air pressure) to compare the Accelerometer derived value.

diyaerospace
The NAR has all their NARAM reports online and many are very good reads.
I am re-reading NARAM-58 by Bryan Sparkman now and he has very good info on calibration and calculations.

Oh, thats me. Yes, that little formula I came up with will work well, and it can even calculate rotation data from the gyro quickly and accurately with only an Arduino Uno.

Since I wrote that, I've made many improvements to the code. I have a Github site with the latest version here: https://github.com/SparkyVT/HPR-Rocket-Flight-Computer

I like the Teensy 3.5/6 and 4.0/1 boards because they have floating point units. This makes quaternion math much faster, which is different from my other little formula.

I also think that you are running your sample rate too low. 50SPS is nothing for an accelerometer. Pump it up to 1000SPS or greater. Use state machine code (or whatever you want to call it) to independently call your barometric sensor since those don't work as fast.

You should not be limited by writing to flash. I use an external SD card and with the SDfat library its wicked fast. My limit is I2C. 85% of every cycle is used in I2C communication with the sensors. I'm able to collect accel/gyro data at 1400SPS, with other sensors at lower rates. Across all the data collected, it comes out to 50,000SPS, or several million data points per flight.

The primary advantage of using an integrated gyro accel is that (hopefully) the sensor axis are well aligned. You can be reasonably certain that the x,y, and z axis of each sensor are nicely orthogonal, but alignment between sensors is less certain. And very less certain if they are in different packages. You can compensate for that but only if you can measure it.

In case anyone forgot, here is my take on a high speed logger. Running a MPU9250 just as fast as it will go.

UPDATE
I have soldered up a new altimeter using some of your suggestions. I am using a KX 134 high gee accelerometer and a BMP 388 pressure sensor. I am using the Arduino nano 33 iot to control all of this and it has a built-in LSM6DS3 IMU that I am using as well. I used this tutorial
https://toptechboy.com/9-axis-imu-lesson-6-determine-tilt-from-3-axis-accelerometer/
To calculate angles using the gyros onboard. My problem is that the angles can become hard to use when the rocket is rolling as the X and Y axis are constantly changing.
Does anyone know a solution to this problem?