Obtaining raw data similar to a MEMS sensor

[brightproject]

To test the IMU algorithm, I wanted to use the FlightGear simulator.

In a nutshell - I receive data in ASCII on the serial port from the simulator using stm32f411, parse the parcel and receive 14 variables with parameters:

<?xml version="1.0"?>

<!-- Example usage: <dash><dash>generic=file,out,50,fgfs.imu,insgns-imu -->

<PropertyList>

 <generic>

  <output>
   <line_separator>newline</line_separator>
   <var_separator>tab</var_separator>

   <!-- <chunk> -->
    <!-- <name>time (sec)</name> -->
    <!-- <type>float</type> -->
    <!-- <format>%.4f</format> -->
    <!-- <node>/sim/time/elapsed-sec</node> -->
   <!-- </chunk> -->

   <chunk>
    <name>roll rate ("p" rad/sec)</name>
    <type>float</type>
    <format>%.6f</format>
    <node>/fdm/jsbsim/velocities/pi-rad_sec</node>
   </chunk>

   <chunk>
    <name>pitch rate ("q" rad/sec)</name>
    <type>float</type>
    <format>%.6f</format>
    <node>/fdm/jsbsim/velocities/qi-rad_sec</node>
   </chunk>

   <chunk>
    <name>yaw rate ("r" rad/sec)</name>
    <type>float</type>
    <format>%.6f</format>
    <node>/fdm/jsbsim/velocities/ri-rad_sec</node>
   </chunk>

   <chunk>
    <name>X accel (body axis) (mps)</name>
    <type>float</type>
    <format>%.4f</format>
    <node>/accelerations/pilot/x-accel-fps_sec</node>
    <factor>0.3048</factor>  <!-- feet to meter -->
   </chunk>

   <chunk>
    <name>Y accel (body axis) (mps)</name>
    <type>float</type>
    <format>%.4f</format>
    <node>/accelerations/pilot/y-accel-fps_sec</node>
    <factor>0.3048</factor>  <!-- feet to meter -->
   </chunk>

   <chunk>
    <name>Z accel (body axis) (mps)</name>
    <type>float</type>
    <format>%.4f</format>
    <node>/accelerations/pilot/z-accel-fps_sec</node>
    <factor>0.3048</factor>  <!-- feet to meter -->
   </chunk>
   
   <chunk>
    <name>Velocity North ("vn" mps)</name>
    <type>float</type>
    <format>%.8f</format>
    <node>/velocities/speed-north-fps</node>
    <factor>0.3048</factor>                  <!-- fps to mps -->
   </chunk>

   <chunk>
    <name>Velocity East ("ve" mps)</name>
    <type>float</type>
    <format>%.8f</format>
    <node>/velocities/speed-east-fps</node>
    <factor>0.3048</factor>                  <!-- fps to mps -->
   </chunk>

   <chunk>
    <name>Velocity Down ("vd" mps)</name>
    <type>float</type>
    <format>%.8f</format>
    <node>/velocities/speed-down-fps</node>
    <factor>0.3048</factor>                  <!-- fps to mps -->
   </chunk>
   
   <chunk>
    <name>yaw angle</name>
    <type>float</type>
    <format>%.5f</format>
    <node>/orientation/heading-deg</node>
 
   </chunk>

   <chunk>
    <name>pitch angle (rad)</name>
    <type>float</type>
    <format>%.5f</format>
    <node>/orientation/pitch-deg</node>
 
   </chunk>

   <chunk>
    <name>roll angle</name>
    <type>float</type>
    <format>%.5f</format>
    <node>/orientation/roll-deg</node>
 
   </chunk>
   
  </output>

 </generic>

</PropertyList>

As far as I understand, the heart of the simulator is the flight data module JSBSim.
Unfortunately, I am not very familiar with either FlightGear or JSBSim.
The problem was described in detail on the forum, please forgive me for creating a similar discussion in the JSBSim community, but these two projects are closely related and perhaps there will be a solution here.
I'm mostly confused about the units the simulation gives when asking for the following parameters:

/fdm/jsbsim/accelerations/a-pilot-x-ft_sec2
/fdm/jsbsim/accelerations/a-pilot-y-ft_sec2
/fdm/jsbsim/accelerations/a-pilot-z-ft_sec2

and

/fdm/jsbsim/velocities/pi-rad_sec
/fdm/jsbsim/velocities/qi-rad_sec
/fdm/jsbsim/velocities/ri-rad_sec

Video demonstrating the strange operation of Mahony's algorithm.

https://www.youtube.com/watch?v=3ecEhKewYBs

I apologize for the quality of the video, sometimes part of the device disappears from view, this is due to filming (I hold the phone with one hand, the mouse with the other, to control the plane).
The video shows that if the movements of the control stick are small, for example towards or away from you, then the attitude indicator also repeats the movements.
If you rock the plane from side to side, just a little, the attitude indicator will display this.
If you fly with a bank of more than 30 degrees, then the bank angle on the attitude indicator begins to jump, from -30 to +30 in roll.
I hope for the help of the community, I myself cannot understand in what units the angular velocities are output by the simulator.
I was hoping to get gyroscope accelerations and angular velocities similar in parameters, like a mems sensor.
If I take data from a real mems sensor, then I get the raw data and multiply it by the LSB and axis sensitivity coefficient.
Usually I get accelerations in G, and angular velocities of the gyroscope in DPS - degrees per second.
According to documentation and file and here also.
For flightgear - accelerations are in G's, and gyro rates are in rads per second's.
But at least I can see 9.8 m/s^2 and understand that the units are 1G.
Regarding the angular velocities of gyroscopes, I am at a loss and not sure what units of measurement they are.
In my code, I had to multiply the raw data by 2 in order for the attitude indicator to move at least somehow, and also add minuses in front of the axies, because the algorithm Mahony generally did not adequately determine orientation angles.

      ax = -x_accel;
      ay = y_accel;
      az = -z_accel;

      gx = roll_rad_sec * 2;
      gy = pitch_rad_sec * 2;
      gz = yaw_rad_sec * 2;

The angular velocities are too low, in a real mems sensor, when rotating the sensor by hand, the speed is about 250 degrees per second or 4.36 radians per second.

https://converter.net/frequency/250-degrees-per-second-to-radians-per-second

But when I'm in flight on the simulator, I change the bank angle, the angular velocity barely reaches 1 radian per second.
Below is an example of the obtained values of angular velocities along the axes at the moment of performing the half-barrel turn.
gyro_x: -0.903614 gyro_y: -0.090636 gyro_z: 0.271146
As far as I understand, flightgear/jbsim does not use the NED reference system, which is used by algorithms for obtaining orientation angles based on mems sensors?

or

Code for the parser and raw data processing using the Mahony algorithm(source):

#define SNPRINTF // Print accel, gyro and RPY
//#define DEBUG // Print all received data
#define SERIAL_BAUD 230400    // Speed uart1
#define BUFF_SIZE 150           // Size of buffer
#define CHUNK 14           // Number blocks from file insgns-imu2.xml
#define BLOCK_END '\t'         // Tab symbol
#define LINE_END '\n'          // End line symbol

float Kp = 0.2; // proportional
float Ki = 0.05; // integral

// Variables for number of chunk
double data[CHUNK];

// Variables for parsing
double roll_rad_sec;
double pitch_rad_sec;
double yaw_rad_sec;
double yaw_angle;
double pitch_angle;
double roll_angle;
double x_accel;
double y_accel;
double z_accel;
double alt_msl;
double QNH;
double QNE;
double vias;
double vtrue;

// from Mahony
double roll;
double pitch;
double yaw;

float ax, ay, az, gx, gy, gz;

// Function of parsing
void parseInput(char* buffer) {
  char* token = strtok(buffer, "\t");
 
  for (int i = 0; i < CHUNK; ++i) {
    data[i] = atof(token);
    token = strtok(NULL, "\t");
  }
}

void setup() {
  Serial.begin(SERIAL_BAUD);
}

void loop() {
  static float deltat = 0;  //loop time in seconds
  static unsigned long now = 0, last = 0; //micros() timers

  if (Serial.available()) { 

    char buffer[BUFF_SIZE];
    int bytesRead = Serial.readBytesUntil(LINE_END, buffer, BUFF_SIZE);
    buffer[bytesRead] = '\0';
   
    parseInput(buffer);

      roll_rad_sec = data[0];
      #ifdef DEBUG
      Serial.print("Gyro X rad/s: ");
      Serial.print(roll_rad_sec, 6);
      // Serial.print(data[0], 7);
      Serial.print(BLOCK_END);
      #endif
      pitch_rad_sec = data[1];
      #ifdef DEBUG
      Serial.print("Gyro Y rad/s: ");
      Serial.print(pitch_rad_sec, 6);
      Serial.print(BLOCK_END);
      #endif
      yaw_rad_sec = data[2];
      #ifdef DEBUG
      Serial.print("Gyro Z rad/s: ");
      Serial.print(yaw_rad_sec, 6);;
      Serial.print(BLOCK_END);
      #endif
      yaw_angle = data[3];
      #ifdef DEBUG
      Serial.print("yaw angle: ");
      Serial.print(yaw_angle, 5);
      Serial.print(BLOCK_END);
      #endif
      pitch_angle = data[4];
      #ifdef DEBUG
      Serial.print("pitch angle: ");
      Serial.print(pitch_angle, 5);
      Serial.print(BLOCK_END);
      #endif
      roll_angle = data[5];
      #ifdef DEBUG
      Serial.print("roll angle: ");
      Serial.print(roll_angle, 5);
      Serial.print(BLOCK_END);
      #endif
      x_accel = data[6];
      #ifdef DEBUG
      Serial.print("Accel X: ");
      Serial.print(x_accel, 4);
      Serial.print(BLOCK_END);
      #endif
      y_accel = data[7];
      #ifdef DEBUG
      Serial.print("Accel Y: ");
      Serial.print(y_accel, 4);
      Serial.print(BLOCK_END);
      #endif
      z_accel = data[8];
      #ifdef DEBUG
      Serial.print("Accel Z: ");
      Serial.print(z_accel, 7);
      Serial.print(BLOCK_END);
      #endif
      alt_msl = data[9];
      #ifdef DEBUG
      Serial.print("Alt Msl: ");
      Serial.print(alt_msl, 1);
      Serial.print(BLOCK_END);
      #endif
      QNH = data[10];
      #ifdef DEBUG
      Serial.print("QNH: ");
      Serial.print(QNH, 2);
      Serial.print(BLOCK_END);
      #endif
      QNE = data[11];
      #ifdef DEBUG
      Serial.print("QNE: ");
      Serial.print(QNE, 2);
      Serial.print(BLOCK_END);
      #endif
      vias = data[12];
      #ifdef DEBUG
      Serial.print("Vel IAS kts: ");
      Serial.print(vias, 6);
      Serial.print(BLOCK_END);
      #endif
      vtrue = data[13];
      #ifdef DEBUG
      Serial.print("Vel True kts: ");
      Serial.print(vtrue, 6);
      Serial.print(LINE_END);
      #endif

      ax = x_accel;
      ay = y_accel;
      az = z_accel;

      gx = roll_rad_sec;
      gy = pitch_rad_sec;
      gz = yaw_rad_sec;

      #ifdef SNPRINTF
      snprintf_master();
      // no engine start on ground
      // gyro_x: -0.000056 gyro_y: 0.000183 gyro_z: -0.000071 accel_x: 0.794700 accel_y: -0.045100 accel_z: -9.789700
      // engine start in air
      // gyro_x: 0.042160 gyro_y: 0.000477 gyro_z: -0.021618 accel_x: 1.808400 accel_y: -0.322700 accel_z: -9.609800
      #endif

      now = micros();
      deltat = (now - last) * 1.0e-6; //seconds since last update
      last = now;

      Mahony_update(ax, ay, az, gx, gy, gz, deltat);

      // Compute Tait-Bryan angles.
      // In this coordinate system, the positive z-axis is down toward Earth.
      // Yaw is the angle between Sensor x-axis and Earth magnetic North
      // (or true North if corrected for local declination, looking down on the sensor
      // positive yaw is counterclockwise, which is not conventional for NED navigation.
      // Pitch is angle between sensor x-axis and Earth ground plane, toward the
      // Earth is positive, up toward the sky is negative. Roll is angle between
      // sensor y-axis and Earth ground plane, y-axis up is positive roll. These
      // arise from the definition of the homogeneous rotation matrix constructed
      // from quaternions. Tait-Bryan angles as well as Euler angles are
      // non-commutative; that is, the get the correct orientation the rotations
      // must be applied in the correct order which for this configuration is yaw,
      // pitch, and then roll.
      // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
      // which has additional links.

      roll  = atan2((q[0] * q[1] + q[2] * q[3]), 0.5 - (q[1] * q[1] + q[2] * q[2]));
      pitch = asin(2.0 * (q[0] * q[2] - q[1] * q[3]));
      //conventional yaw increases clockwise from North. Not that the MPU-6050 knows where North is.

      // yaw   = -atan2((q[1] * q[2] + q[0] * q[3]), 0.5 - (q[2] * q[2] + q[3] * q[3]));
      // yaw   = atan2((q[1] * q[2] + q[0] * q[3]), (q[2] * q[2] + q[3] * q[3]));
      yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);

      // yaw   *= 180.0 / PI;
      // pitch *= 180.0 / PI;
      // roll *= 180.0 / PI;
  }
}

//--------------------------------------------------------------------------------------------------
// Mahony scheme uses proportional and integral filtering on
// the error between estimated reference vector (gravity) and measured one.
// Madgwick's implementation of Mayhony's AHRS algorithm.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date      Author      Notes
// 29/09/2011 SOH Madgwick    Initial release
// 02/10/2011 SOH Madgwick  Optimised for reduced CPU load
// last update 07/09/2020 SJR minor edits
//--------------------------------------------------------------------------------------------------
// IMU algorithm update

void Mahony_update(float ax, float ay, float az, float gx, float gy, float gz, float deltat) {
  float recipNorm;
  float vx, vy, vz;
  float ex, ey, ez;  //error terms
  float qa, qb, qc;
  static float ix = 0.0, iy = 0.0, iz = 0.0;  //integral feedback terms
  float tmp;

  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
  // https://www.collimator.ai/reference-guides/how-to-normalize-the-vector
  tmp = ax * ax + ay * ay + az * az;
  if (tmp > 0.0)
  {

    // Normalise accelerometer (assumed to measure the direction of gravity in body frame)
    recipNorm = 1.0 / sqrt(tmp);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;

    // Estimated direction of gravity in the body frame (factor of two divided out)
    vx = q[1] * q[3] - q[0] * q[2];
    vy = q[0] * q[1] + q[2] * q[3];
    vz = q[0] * q[0] - 0.5f + q[3] * q[3];

    // Error is cross product between estimated and measured direction of gravity in body frame
    // (half the actual magnitude)
    ex = (ay * vz - az * vy);
    ey = (az * vx - ax * vz);
    ez = (ax * vy - ay * vx);

    // Compute and apply to gyro term the integral feedback, if enabled
    if (Ki > 0.0f) {
      ix += Ki * ex * deltat;  // integral error scaled by Ki
      iy += Ki * ey * deltat;
      iz += Ki * ez * deltat;
      gx += ix;  // apply integral feedback
      gy += iy;
      gz += iz;
    }

    // Apply proportional feedback to gyro term
    gx += Kp * ex;
    gy += Kp * ey;
    gz += Kp * ez;
  }

  // Integrate rate of change of quaternion, q cross gyro term
  deltat = 0.5 * deltat;
  gx *= deltat;   // pre-multiply common factors
  gy *= deltat;
  gz *= deltat;
  qa = q[0];
  qb = q[1];
  qc = q[2];
  q[0] += (-qb * gx - qc * gy - q[3] * gz);
  q[1] += (qa * gx + qc * gz - q[3] * gy);
  q[2] += (qa * gy - qb * gz + q[3] * gx);
  q[3] += (qa * gz + qb * gy - qc * gx);

  // renormalise quaternion
  recipNorm = 1.0 / sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
  q[0] = q[0] * recipNorm;
  q[1] = q[1] * recipNorm;
  q[2] = q[2] * recipNorm;
  q[3] = q[3] * recipNorm;
}

void snprintf_master(){
  char text[250];

  char gyro_x_str[10];
  char gyro_y_str[10];
  char gyro_z_str[10];
  char accel_x_str[10];
  char accel_y_str[10];
  char accel_z_str[10];
  char roll_str[10];
  char pitch_str[10];
  char yaw_str[10];

  dtostrf(roll_rad_sec, 6, 6, gyro_x_str);
  dtostrf(pitch_rad_sec, 6, 6, gyro_y_str);
  dtostrf(yaw_rad_sec, 6, 6, gyro_z_str);
  dtostrf(x_accel, 6, 6, accel_x_str);
  dtostrf(y_accel, 6, 6, accel_y_str);
  dtostrf(z_accel, 6, 6, accel_z_str);
  dtostrf(roll, 6, 6, roll_str);
  dtostrf(pitch, 6, 6, pitch_str);
  dtostrf(yaw, 6, 6, yaw_str);

  snprintf(text, sizeof(text), "gyro_x: %s gyro_y: %s gyro_z: %s accel_x: %s accel_y: %s accel_z: %s roll: %s pitch: %s yaw: %s", gyro_x_str, gyro_y_str, gyro_z_str, accel_x_str, accel_y_str, accel_z_str, roll_str, pitch_str, yaw_str);

  Serial.println(text);
}

I would be grateful for help and tips on how to correctly scale the raw data of accelerations and angular velocities of the gyroscope from the simulator, as well as the directions of these axes.

[seanmcleod]

For input to your IMU algorithm I'd suggest looking at using:

jsbsim/src/models/FGPropagate.cpp

Lines 833 to 835 in 56c6662

	PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
	PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
	PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);

jsbsim/src/models/FGAccelerations.cpp

Lines 356 to 358 in 56c6662

	PropertyManager->Tie("accelerations/udot-ft_sec2", this, eU, (PMF)&FGAccelerations::GetUVWdot);
	PropertyManager->Tie("accelerations/vdot-ft_sec2", this, eV, (PMF)&FGAccelerations::GetUVWdot);
	PropertyManager->Tie("accelerations/wdot-ft_sec2", this, eW, (PMF)&FGAccelerations::GetUVWdot);

The units are pretty self explanatory given their encoding in the property name.

[brightproject]

For input to your IMU algorithm I'd suggest looking at using:

Thank you very much @seanmcleod .
I found these properties in the simulator:

As far as I understand, these are accelerations of the center of mass of the body, similar to 9.81 m/s^2 or 1G?
Perhaps you know what the ratio of the UVW and XYZ axis is?
The directions of the acceleration axes and rotation of the gyroscopes are not yet clear to me.
It is interesting, however, that these parameters are tied to the structural or body frame🙂?
P.S. Found also your explanation here.

[seanmcleod]

Hmm, on second thoughts, what might actually be better input to an IMU algorithm that is trying to mimic the input from a real 3-axis gyro and 3-axis accelerometer is to make use of the gyro and accelerometer sensors in JSBSim.

https://github.com/JSBSim-Team/jsbsim/blob/56c666265ac1db3216cc76c7980a87ec424a6546/src/models/flight_control/FGGyro.h

https://github.com/JSBSim-Team/jsbsim/blob/56c666265ac1db3216cc76c7980a87ec424a6546/src/models/flight_control/FGAccelerometer.h

You can take a look at the equivalent .cpp files to see their implementation.

UVW are velocities in the body frame.

[seanmcleod]

In terms of frames of reference, you'll also see UVW velocities etc. mentioned take a look at - https://jsbsim-team.github.io/jsbsim-reference-manual/user/concepts/frames-of-reference/

[brightproject]

In terms of frames of reference, you'll also see UVW velocities etc. mentioned take a look at - https://jsbsim-team.github.io/jsbsim-reference-manual/user/concepts/frames-of-reference/

This is valuable information collected in such a convenient form.
For some reason I didn’t come across it before, thank you.
My dream is to find a like-minded man, who has the ability to deal with microcontrollers and understands JSBSim simulation and flies in FlightGear.
Sometimes simple obstacles prevent you from moving forward, and now I have stopped at a simple place, due to lack of understanding.
It would be nice to see an example of someone receiving data from a simulation, as if from a real mems sensor.
A lot would be clear right away and things would move forward.
Now I'm kind of getting the data, converting it from feet to meters.
But the algorithm doesn't work as intended.
Therefore, there are still doubts about the correctness of the data obtained from the simulation.

You may know that you can somehow change the angle of roll and pitch on a non-moving aircraft.
Those. so that it was not in flight.
In JSBSim or FlightGear?
Perhaps some configuration can be used to set the aircraft to rotate while parked, first along one axis (left-right), and then along the other (back-forward).
I could test my algorithm statically, and then in flight.
On a real device, everything also works fine for me as long as I rotate the device in my hand.

https://youtu.be/8kvTS-uUStA

But in flight centrifugal forces act and everything works poorly.

https://youtu.be/OXhmy1ul1sw

[seanmcleod]

But in flight centrifugal forces act and everything works poorly.

So currently your implementation doesn't work well with actual accelerometers and gyros in a real aircraft, so the issue you're having isn't specific to JSBSim simulation.

I'd suggest testing your implementation using some sample data from here and confirm that you can get your implementation to work with this data first.

https://github.com/xioTechnologies/Fusion

Two example Python scripts, simple_example.py and advanced_example.py are provided with example sensor data to demonstrate use of the package.

https://github.com/xioTechnologies/Fusion/blob/main/Python/sensor_data.csv

[seanmcleod]

You could always use something like this - https://www.adafruit.com/product/4754 that implements the IMU algorithm and simply spits out the calculated attitude.

[brightproject]

You could always use something like this - https://www.adafruit.com/product/4754 that implements the IMU algorithm and simply spits out the calculated attitude.

This is a greatest breadboard.

I have one, but I want to write my own fusion algorithm, for this I am studying aerodynamics and many other things.
Today I’m testing fusion from xioTechnologies on flights in FlightGear.

#include <Fusion.h>
#include <stdbool.h>
#include <stdio.h>

// #define PRINTF

#define SAMPLE_PERIOD (0.01f) // replace this with actual sample period

#define SERIAL_BAUD 115200

FusionAhrs ahrs;

void setup() {
	
	Serial.begin(SERIAL_BAUD);// PA9, PA10
	
	FusionAhrsInitialise(&ahrs);

}

void loop() { // this loop should repeat each time new gyroscope data is available
        const FusionVector gyroscope = {0.2f, 1.3f, 0.0f}; // replace this with actual gyroscope data in degrees/s
        const FusionVector accelerometer = {0.0f, 0.0f, 1.0f}; // replace this with actual accelerometer data in g

        FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, SAMPLE_PERIOD);

        const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));
		
		#ifdef PRINTF

        printf("Roll %0.1f, Pitch %0.1f, Yaw %0.1f\n", euler.angle.roll, euler.angle.pitch, euler.angle.yaw);
		
		#else
		
		Serial.print(" \tEiler Roll: ");
		Serial.print(euler.angle.roll);
		Serial.print(" \tEiler Pitch: ");
		Serial.print(euler.angle.pitch);
		Serial.print(" \tEiler Yaw: ");
		Serial.println(euler.angle.yaw);
	  
		#endif
}

Their is a simple version that uses only a gyroscope and an accelerometer.
Despite a more complex filter than a simple Madgwick - a filter from xioTechnologies in a simple implementation, also cannot distinguish between gravity and centrifugal acceleration.
And in a roll, the angle of orientation of this roll remains correct for only a few seconds.
I also plan to compile an example of fusion using a magnetometer, but I really don’t know how to get it from the simulation.
I’m also still having difficulty getting GPS NED speeds.
Could you tell me which property tree they are in?
Thank you for your participation @seanmcleod

[seanmcleod]

I’m also still having difficulty getting GPS NED speeds.

jsbsim/src/models/FGPropagate.cpp

Lines 825 to 827 in 56c6662

	PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
	PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
	PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);

...also cannot distinguish between gravity and centrifugal acceleration.

When I looked into IMU algorithms many years ago briefly, I thought what they typically did, assuming only accelerometers and gyros was to reset the attitude periodically based only on the accelerometers when the total acceleration was close enough to 1g, which would get rid of drift in the attitude solution due to integrating noisy gyro data. But when the total acceleration was greater than 1g it would switch back to integrating the gyro rates. Possibly also making use of a Kalman filter, and then also in terms of more fusion making use of a magnetometer, GPS velocities, air data velocity etc.

[brightproject]

Are these exactly the velNED speeds that any GPS unit adapter produces using the ubx protocol?

jsbsim/src/models/FGPropagate.cpp

Lines 825 to 827 in 56c6662

	PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
	PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
	PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);

[brightproject]

I'd suggest testing your implementation using some sample data from here and confirm that you can get your implementation to work with this data first.

https://github.com/xioTechnologies/Fusion

I succeeded to test a simple implementation of the xioTechnologies fusion sensor.
https://www.youtube.com/watch?v=cWlsb3izkYQ
GUI on Processing from here with minor modifications..

At rolls of up to 10 degrees, the algorithm handles the evolutions quite well.
But if a prolonged roll begins, then the roll value calculated using the xioTechnologies algorithm first advances ahead of the real roll angle, which is displayed by the PFD Airbus A320, and then begins to return to zero.
And when the real roll angle (on the PFD) begins to move in the opposite direction, i.e. the plane levels out in the horizon, the roll value calculated by the xioTechnologies algorithm begins to move in the opposite direction, aggravating the situation.
The reason for this is centrifugal acceleration, which cannot be distinguished from gravity.
The situation with the pitch angle is approximately the same.
The calculated yaw angle does not correspond to reality at all - the reason is that there is no magnetometer.

[seanmcleod]

What JSBSim properties did you use/feed into the algorithm? Were they the outputs from the FGGyro and FGAccelerometer sensor models?

In terms of https://github.com/xioTechnologies/Fusion/blob/main/README.md#algorithm-settings what settings values did you use, in particular for:

• gain
• accelerationRejection
• recoveryTriggerPeriod

Given that you're getting perfect gyro data, unless you configured the FGGyro sensor with noise, bias, drift etc. then you could for example for the simulator case set gain very close to 0.

What is also useful are the algorithm internal states and flags that let you know what the algorithm is doing for each sample.

If you can't get this algorithm to work well enough for a flight application via the various settings then your other option is to look at say the algorithm used by something like ArduPilot.

[brightproject]

What JSBSim properties did you use/feed into the algorithm? Were they the outputs from the FGGyro and FGAccelerometer sensor models?

In terms of https://github.com/xioTechnologies/Fusion/blob/main/README.md#algorithm-settings what settings values did you use, in particular for:

• gain
• accelerationRejection
• recoveryTriggerPeriod

Given that you're getting perfect gyro data, unless you configured the FGGyro sensor with noise, bias, drift etc. then you could for example for the simulator case set gain very close to 0.

What is also useful are the algorithm internal states and flags that let you know what the algorithm is doing for each sample.

If you can't get this algorithm to work well enough for a flight application via the various settings then your other option is to look at say the algorithm used by something like ArduPilot.

I tried different parameters from, but did not notice any significant differences.

void FusionAhrsInitialise(FusionAhrs *const ahrs) {
    const FusionAhrsSettings settings = {
            .convention = FusionConventionNwu,
            // .convention = FusionConventionEnu,
            // .convention = FusionConventionNed,
            .gain = 0.5f,
            // .gain = 0.01f,
            .gyroscopeRange = 0.0f,
            // .gyroscopeRange = 250.0f,
            // .gyroscopeRange = 500.0f,
            // .gyroscopeRange = 1000.0f,
            // .gyroscopeRange = 2000.0f,
            .accelerationRejection = 10.0f,
            // .accelerationRejection = 0.0f,
            // .magneticRejection = 90.0f,
            .magneticRejection = 0.0f,
            .recoveryTriggerPeriod = 10,
    };
    FusionAhrsSetSettings(ahrs, &settings);
    FusionAhrsReset(ahrs);
}

Probably, this is still a limitation of the algorithm’s capabilities based only on the accelerations and angular velocities of the gyroscope.
I'll try using a magnetometer.
P.S. Or, according to your information, using the fusion algorithm from xioTechnologies it is possible to achieve an acceptable roll angle retention even using only acceleration and angular velocity of the gyroscope?

[seanmcleod]

@brightproject you didn't answer my question about what JSBSim properties you were feeding into xioTechnologies algorithm.

So I spent a little bit of time trying it out myself. First off, have you run their https://github.com/xioTechnologies/Fusion/blob/main/Python/advanced_example.py? It's very useful in terms of showing the state of the algorithm for each time step.

So I added FGAccelerometers at the cg and also added FGGyros into the 737 aircraft in JSBSim.

	<system name="Sensors">
		<channel name="accelerometers">
			<accelerometer name="accelerometer_X">
				<location unit="IN">
					<x> 610.8 </x>
					<y> 0   </y>
					<z> -35.1 </z>
				</location>
				<axis> X </axis>
				<output> fcs/accelerometer/X </output>
			</accelerometer>
			<accelerometer name="accelerometer_Y">
				<location unit="IN">
					<x> 610.8 </x>
					<y> 0   </y>
					<z> -35.1 </z>
				</location>
				<axis> Y </axis>
				<output> fcs/accelerometer/Y </output>
			</accelerometer>
			<accelerometer name="accelerometer_Z">
				<location unit="IN">
					<x> 610.8 </x>
					<y> 0   </y>
					<z> -35.1 </z>
				</location>
				<axis> Z </axis>
				<output> fcs/accelerometer/Z </output>
			</accelerometer>
		</channel>
		<channel name="gyros">
			<gyro name="fcs/gyro/X">
				<axis> X </axis>
			</gyro>
			<gyro name="fcs/gyro/Y">
				<axis> Y </axis>
			</gyro>
			<gyro name="fcs/gyro/Z">
				<axis> Z </axis>
			</gyro>
		</channel>
	</system>

Then wrote some python code to trim the 737, let it fly for 30s and then add an elevator step input and then again at 60s add an additional elevator step input and let the aircraft fly for 180s.

while fdm.get_sim_time() < 180:
    fdm.run()
    time_s.append(fdm.get_sim_time())

    if fdm.get_sim_time() > 30:
        fdm['fcs/elevator-cmd-norm'] = -0.1
    if fdm.get_sim_time() > 60:
        fdm['fcs/elevator-cmd-norm'] = -0.2

    pitch_s.append(math.degrees(fdm['attitude/theta-rad']))
    roll_s.append(math.degrees(fdm['attitude/phi-rad']))

    ax = fdm['fcs/accelerometer/X']
    ay = fdm['fcs/accelerometer/Y']
    az = fdm['fcs/accelerometer/Z']

    amag = math.sqrt(ax**2 + ay**2 + az**2)

    ax_s.append(ax/g)
    ay_s.append(ay/g)
    az_s.append(az/g)
    amag_s.append(amag/g)

    gyrox = fdm['fcs/gyro/X']
    gyroy = fdm['fcs/gyro/Y']
    gyroz = fdm['fcs/gyro/Z']

    gyrox_s.append(math.degrees(gyrox))
    gyroy_s.append(math.degrees(gyroy))
    gyroz_s.append(math.degrees(gyroz))
    
    f.write(f'{fdm.get_sim_time()},{math.degrees(gyrox)},{math.degrees(gyroy)},{math.degrees(gyroz)},{-ax/g},{ay/g},{-az/g}')
    f.write('\n')

Note the sign changes for ax and az to match their default NWU convention. Only dealing with a pitch input for the initial test.

Then I plot JSBSim's results.

Then I modified their advanced_example.py to ignore the magnetometer since I don't output any data for it etc. and get this output to compare to the JSBSim ground truth for pitch angle.

Now in order to get such a close match I had to bump the gain way down to 0.01 from the default of 0.5. Here are some of the relevant snippets. Note the recovery trigger period is in terms of number of samples, not seconds. I noticed you had used a figure of 5 above, and I'm assuming you assumed that meant 5s.

sample_rate = 120  # 120 Hz

timestamp = data[:, 0]
gyroscope = data[:, 1:4]
accelerometer = data[:, 4:7]
#magnetometer = data[:, 7:10]

# Instantiate algorithms
#offset = imufusion.Offset(sample_rate)
ahrs = imufusion.Ahrs()

ahrs.settings = imufusion.Settings(imufusion.CONVENTION_NWU,  # convention
                                   0.01,  # gain
                                   2000,  # gyroscope range
                                   10,  # acceleration rejection
                                   10,  # magnetic rejection
                                   5 * sample_rate)  # recovery trigger period = 5 seconds
...
    #ahrs.update(gyroscope[index], accelerometer[index], magnetometer[index], delta_time[index])
    ahrs.update_no_magnetometer(gyroscope[index], accelerometer[index], delta_time[index])  

So with a gain of 0.01 it means you're assuming your gyros are very good in terms of bias, drift, noise etc. which they are in my case, since I haven't configured bias, drift, noise etc. for the FGGyro it means they're perfect gyros outputting ground truth angular velocities.

Just some very initial testing I've done this weekend. Will do some more to analyse their algorithm, generate more JSBSim test data etc. to use in the analysis etc.

[seanmcleod]

And as a quick example to show how and when the accelerometer rejection and recovery etc. kicks in, I've changed the gain to 0.1 and the acceleration rejection to 2, with the following result.

[brightproject]

what JSBSim properties you were feeding into xioTechnologies algorithm.

I wanted to answer, but I forgot and answered with a different phrase, please forgive me and I get the following properties from the FlightGear simulator:

/accelerations/pilot/x-accel-fps_sec
/accelerations/pilot/у-accel-fps_sec
/accelerations/pilot/z-accel-fps_sec

and

/fdm/jsbsim/velocities/pi-rad_sec
/fdm/jsbsim/velocities/qi-rad_sec
/fdm/jsbsim/velocities/ri-rad_sec

Now I managed to collect some data received from my microcontroller:

Left roll, descent, right roll, climb.
Blue line - the algorithm calculates, green line - I receive angle data from the simulator.

Please note how the value of the roll angle or pitch angle calculated by the algorithm on my microcontroller differs from the output orientation angles of the simulator.

First, the roll angle is calculated similarly to the obtained roll angle from the simulator.
But halfway through the journey, the calculated roll angle begins to return to its original value, and the return is not linear, but as if the PID is working.
When the roll angle obtained from the simulator decreases, the calculated angle for some reason begins to move in the opposite direction.

Either I messed up the signs of the original axes, or this is an incorrect setting of the algorithm...

Raw data sampling from aviasim FlightGear - acceleration in m/s/s, gyro rates in rad/sec.
I convert the data into the xioTechnologies.

      ax = -x_accel; // 9.81 m/s/s
      ay = y_accel; // 9.81 m/s/s
      az = -z_accel; // 9.81 m/s/s

      ax /= G;
      ay /= G;
      az /= G;

      gx = roll_rad_sec; // rad/sec
      gy = pitch_rad_sec; // rad/sec
      gz = yaw_rad_sec; // rad/sec

      gx *= RAD_TO_DEG; // deg/sec
      gy *= RAD_TO_DEG; // deg/sec
      gz *= RAD_TO_DEG; // deg/sec

Unfortunately, it’s still not easy for me to understand how the JSBSim simulation works, and I use FlightGear because with it I was able to get at least some data.
I'd like to get into how you control the simulation JSBSim, but I think I'll do that a little later.
I really should try to figure it out with an example.
Thank you, as soon as there is new information, I will write here.