Question about sensor filtering

The Rocketry Forum

Help Support The Rocketry Forum:

This site may earn a commission from merchant affiliate links, including eBay, Amazon, and others.
Interesting. How exactly would that work?

For example. You have a 20 degree clockwise rotation about x-axis. Rocket then rotates 180 degrees about z-axis. Then there is a continuing clockwise rotation about x-axis.

What is the gyro accumulating scheme to get the attitude of the z-axis ?
I guess I would still need either a rotation matrix or quaternion?
 
My one concern with that is I do want to have an orientation track for multi-stage rockets, where I really want to have an attitude lockout so S2 won't ignite if it's on its side.
Might be better to use a 9 DOF device, adding a 3 axis magnetometer.
 
and you don't violate whatever COCOM rules are built into the GPS' firmware. I have found that flights of 10K+ generally produce reasonable GPS altitude readings after apogee... for shorter/lower flights, not so much.
Just a tiny nit-pick: the limits these days are probably ITAR based. COCOM died nearly 30 years ago.

TP
 
Might be better to use a 9 DOF device, adding a 3 axis magnetometer.
Mags don't work very well in flight... they ARE good for getting your initial compass direction, assuming that any metal in your AV bay isn't honking up the mag readings.
 
Gyros will experience drift though right? As far as I'm aware a Kalman filter uses the accelerometer to correct for this using earth reference, so how is gyro drift corrected without that reference frame?
The gyro drift is somewhat temperature dependent. Ardupilot has a calibration algorithm that works by looking at the gyro rate when known stationary and temperature is ramping. Gyro cal mode is entered. The unit is then put in an oven set at about 70degC. The cal routine monitors the gyro rates as the temperature of the chip rises, logging the values along the way. It beeps when complete. Then during flight the relevant inverse rates are interpolated and subtracted from the measurements to give a corrected final value much closer to zero.
 
I guess I would still need either a rotation matrix or quaternion?
My main point—and I did have one—was to always try to rely on fast, simple integer math. It's essentially free and it can reduce the amount of processing power you need as well as power consumption of the ultimate solution.

In this case I suppose you could think of it as a poor man's quaternion, and I guess it is, but really it's a fast, running approximation of tilt.

Assuming Z is up, we don't ultimately care about its value per se, but it does affect how we accumulate the subsequent X and Y rotations to figure out tilt, which is ultimately combination of those.

Use an integer look up table for the trig (table size dependent on accuracy desired and the ultimate size of integers), and after every step calculate the new X and Y rotations relative to launch. But keep it all in integers.

Double bonus points if you can avoid using POW and SQRT math, though these functions aren't too bad compared to the trig ones. (SQRT is a surprisingly fast thing on most microcontrollers/compilers, for instance.)
 
My main point—and I did have one—was to always try to rely on fast, simple integer math. It's essentially free and it can reduce the amount of processing power you need as well as power consumption of the ultimate solution.

In this case I suppose you could think of it as a poor man's quaternion, and I guess it is, but really it's a fast, running approximation of tilt.

Assuming Z is up, we don't ultimately care about its value per se, but it does affect how we accumulate the subsequent X and Y rotations to figure out tilt, which is ultimately combination of those.

Use an integer look up table for the trig (table size dependent on accuracy desired and the ultimate size of integers), and after every step calculate the new X and Y rotations relative to launch. But keep it all in integers.

Double bonus points if you can avoid using POW and SQRT math, though these functions aren't too bad compared to the trig ones. (SQRT is a surprisingly fast thing on most microcontrollers/compilers, for instance.)
How quickly does calculation error accumulate with this method versus quaternions?
 
In this case I suppose you could think of it as a poor man's quaternion, and I guess it is, but really it's a fast, running approximation of tilt.

Assuming Z is up, we don't ultimately care about its value per se, but it does affect how we accumulate the subsequent X and Y rotations to figure out tilt, which is ultimately combination of those.
instance.)
I can't imagine (probably my limitation) on how this can probably work. I can see how it could work if z-rate is zero, but if there is rotation around z while x and y are rotating this becomes a very complex calculation.

I'd love to see some pseudocode on this idea. I did a quick search and did not find anything on this method. It would be bleeding edge if it can work.
 
Last edited:
Hardware floating point is also a wonderful thing.
Yes it was, the 8087, 80287, 68881, 68882 etc. were great. Unfortunately, they weren't available yet when I started with embedded controllers and SBCs with 8051 and 6801 uCs. At least EEPROM were there so the UV erasers worked great even if they did take a long time. When I got to use 16K EEPROMS, that was great! So much memory! Much better than the older 4K and 8K chips.

Of course the hardware and firmware was always way slower than what you really wanted so learning how to do things faster was at a premium and without hardware floating point, doing as much as possible with integer math was pretty much required. Writing and using assembler routines helped a lot too.
 
Last edited:
Yes it was, the 8087, 80287, 68881, 68882 etc. were great. Unfortunately, they weren't available yet when I started with embedded controllers and SBCs with 8051 and 6801 uCs. At least EEPROM were there so the UV erasers worked great even if they did take a long time. When I got to use 16K EEPROMS, that was great! So much memory! Much better than the older 4K and 8K chips.

Of course the hardware and firmware was always way slower than what you really wanted so learning how to do things faster was at a premium and without hardware floating point, doing as much as possible with integer math was pretty much required. Writing and using assembler routines helped a lot too.
Back in the 80's I got a lot of mileage out the 6502 family. It does not even have a multiply instruction. The fastest way to multiply was by table look up. But yes, IEEE floating point math is good thing especialy when achieved in hardware, and even more so with pipelined execution. I'm not sure what is going on inside contemporary micro-controllers these days, but the game was always to find the cheapest solution that that would do what you needed.
 
I can't imagine (probably my limitation) on how this can probably work. I can see how it could work if z-rate is zero, but if there is rotation around z while x and y are rotating this becomes a very complex calculation.

I'd love to see some pseudocode on this idea. I did a quick search and did not find anything on this method. It would be bleeding edge if it can work.

This is not something I've implemented or tested, I just pass it along here in the spirit of demonstrating integer math.
This example uses the assumption of 1/100 seconds sampling of a 250 dps full rate gyro.
Can be modified for other sample rates and gyros.
Use at your own risk, I just whipped this up today for this example.


#define Q_SCALE 16384 // Scaling factor for 16-bit quaternions

// Structure to hold the quaternion components
typedef struct {
int16_t w, x, y, z;
} quaternion_t;

int main() {
// Initialize quaternion to represent rocket at rest
quaternion_t q = { Q_SCALE, 0, 0, 0 };
// Read gyroscope every 0.01 seconds and update quaternion

while (1) {

// Every 0.01 seconds
int16_t x_rate = read_x_rate();
int16_t y_rate = read_y_rate();
int16_t z_rate = read_z_rate();
uint16_t dt = 10; // Time step in milliseconds
update_quaternion(&q, x_rate, y_rate, z_rate, dt);
int32_t tilt = calculate_tilt(&q);
// Do something with tilt

}
}

// Function to update the quaternion based on gyroscope readings
void update_quaternion(quaternion_t* q, int16_t x_rate, int16_t y_rate, int16_t z_rate, uint16_t dt) {
const int32_t GYRO_SCALE = 32768 / 250; // Scaling factor for gyroscope (use yours)
const int32_t DT_SCALE = 100; // Scaling factor for time step

int32_t w = q->w, x = q->x, y = q->y, z = q->z;
int32_t dt_scaled = dt * DT_SCALE;
int32_t x_rate_scaled = x_rate * GYRO_SCALE;
int32_t y_rate_scaled = y_rate * GYRO_SCALE;
int32_t z_rate_scaled = z_rate * GYRO_SCALE;

// Calculate incremental quaternion from gyroscope readings
int32_t angle = isqrt(x_rate_scaled * x_rate_scaled + y_rate_scaled * y_rate_scaled + z_rate_scaled * z_rate_scaled);
if (angle == 0) {
// No rotation, so no update needed
return;
int32_t sin_half_angle = isqrt(((Q_SCALE << 16) - angle) * angle) >> 8;
int32_t cos_half_angle = (Q_SCALE << 15) - sin_half_angle * sin_half_angle;
int32_t x_incremental = (sin_half_angle * x_rate_scaled) / angle;
int32_t y_incremental = (sin_half_angle * y_rate_scaled) / angle;
int32_t z_incremental = (sin_half_angle * z_rate_scaled) / angle;
int32_t w_incremental = cos_half_angle;
}
// Update quaternion
q->w = ((w * w_incremental - x * x_incremental - y * y_incremental - z * z_incremental) >> 14) + w;
q->x = ((w * x_incremental + x * w_incremental + y * z_incremental - z * y_incremental) >> 14) + x;
q->y = ((w * y_incremental - x * z_incremental + y * w_incremental + z * x_incremental) >> 14) + y;
q->z = ((w * z_incremental + x * y_incremental - y * x_incremental + z * w_incremental) >> 14) + z;
}

// Function to calculate tilt from quaternion
int32_t calculate_tilt(const quaternion_t* q) {
const int32_t TILT_SCALE = 8192; // Scaling factor for tilt angle
int32_t q_w = q->w, q_x = q->x, q_y = q->y, q_z = q->z;
int32_t q_x_scaled = (q_x * TILT_SCALE) / q_w;
int32_t q_w_scaled = (q_w * TILT_SCALE) / q_w;
int32_t tilt = iatan2(q_x_scaled, q_w_scaled) * 2;
return tilt;
}

// Function to estimate the inverse square root of an integer
int32_t isqrt(int32_t x) {
int32_t y = x;
int32_t z = (x + 1) >> 1;
while (z < y) {
y = z;
z = (x / z + z) >> 1;
}
return y;
}

// Function to estimate the arctangent of a ratio of integers
int32_t iatan2(int32_t y, int32_t x) {
const int32_t ATAN2_BITS = 14;
const int32_t ATAN2_MASK = (1 << ATAN2_BITS) - 1;
const int32_t ATAN2_COUNT = (1 << ATAN2_BITS) + 1;

int32_t angle, abs_y, r, scaled_y, scaled_x;
if (x == 0) {
angle = (y > 0) ? 90 : -90;
} else {
abs_y = (y < 0) ? -y : y;
r = ((x < 0) ? (x + ATAN2_MASK) : x) >> (15 - ATAN2_BITS);
scaled_y = abs_y >> (15 - ATAN2_BITS);
scaled_x = r;
angle = (scaled_x == 0) ? ((y >= 0) ? 90 : -90) :
((scaled_y * scaled_y) <= (scaled_x * scaled_x)) ?
(r >= 0 ? 0 : 180) + ((r * scaled_y + (1 << (ATAN2_BITS - 1))) >> ATAN2_BITS) :
(y >= 0 ? 90 : -90) - ((r * scaled_x + (1 << (ATAN2_BITS - 1))) >> ATAN2_BITS);
}
return angle;
}

 
Get some sample data (or record your own) and play around with processing in Matlab or Octave. Much easier to debug and test things out. The quaternion math is actually surprisingly easy once you work through it. There is a reason they are popular for such applications.
 
Attached Zip file contains data from 6 flight in .csv format.
All the Data has been calibrated to correct for offset and gain errors.
Header gives what the column data is. All units are metric.
A = Accelerometer, G = Gyro, M = Magnetometer, and Altitude.
Then integrated Acc to Velocity & Altitude and Gyro rate to angle.
If any question then please ask.
 

Attachments

  • Caled.zip
    521.2 KB · Views: 0
I do not know how much noise there is in your sensor data. For something I ended up writing in a hurry - in transit so to speak - the sensor data was glitchy for various reasons. I ended up throwing in outlier rejection and alpha-beta tracking on each sensor input, and also applying a bit of a physics model (helps with outlier rejection), to clean up the sensor data. I then used the cleaned up data for the actual tracking - projecting forward in time the small amount necessary to compensate for the lag of doing this sort of filtering. You do that if you need to know where you are, rather than where you were when the sensor readings were taken.

Asynchronous sensor readings make it more fun.

I ended up writing the software from scratch overnight while everyone else was sleeping. Some of my finest emergency work!

I won't say what it was for or why we needed it, but the final drift was consistently on the order of 1000yds over 8 hours of moving around. Not too bad when you don't have GPS available and aren't working with state of the art equipment. It gets you in the ballpark.

Sorry, no, the code is unavailable.

For rocket applications, about the axis of the rocket, watch out for the nyquist limit.

Gerald
 
I'm not totally sure that I understand how this is possible, but I seem to have an extremely janky quaternion orientation solver using 208hz FIFO and running in under 5ms on the Atmega. The sources (AKA mildly stolen code) are this thread and this Wikipedia article. My calibration has issues, it seems the offset value is varying over time so I'm going to collect data of gyro readings and temperature and see if there's a correlation that I can correct out in the loop.

There's definitely some optimizations I can make, which I will try to do and update in this thread!
 
Last edited:
Back
Top