mekf.h File Reference

Multiplicative Extended Kalman Filter (MEKF) for attitude and gyroscope bias estimation. More...

#include "rtwtypes.h"
#include <stddef.h>
#include <stdlib.h>

Include dependency graph for mekf.h:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Macros
#define	HIGHEST_NUMBER   4294967294

Functions
void	mekf (const double q[4], const double beta[3], double P[36], const double omega[3], const double b_B[3], double height_AGL, double q_new[4], double beta_new[3], double P_new[36])
	Executes one prediction and correction step of the MEKF.

Detailed Description

Multiplicative Extended Kalman Filter (MEKF) for attitude and gyroscope bias estimation.

Author

Francesco Abate, MSA (Originally generated by MATLAB Coder)

Version

24.2 (MATLAB Coder version)

Date

2025-05-05

This header file declares the mekf function, an auto-generated MEKF from MATLAB Coder. The filter is designed to determine the rocket's attitude (3D orientation) by fusing high-frequency gyroscope data with lower-frequency magnetometer readings.

It estimates a 6-degree-of-freedom state:

• 3 degrees for attitude (represented by a quaternion).
• 3 degrees for gyroscope bias.

This is a core component of the rocket's guidance, navigation, and control (GNC) system.

Definition in file mekf.h.

Macro Definition Documentation

HIGHEST_NUMBER

#define HIGHEST_NUMBER   4294967294

Definition at line 34 of file mekf.h.

Function Documentation

mekf()

void mekf	(	const double	q[4],
		const double	beta[3],
		double	P[36],
		const double	omega[3],
		const double	b_B[3],
		double	height_AGL,
		double	q_new[4],
		double	beta_new[3],
		double	P_new[36]
	)

Executes one prediction and correction step of the MEKF.

This function propagates the state forward using gyroscope readings (prediction step) and then corrects the estimate using magnetometer data (correction step).

Parameters

[in,out]	P	Estimate Covariance Matrix [6x6]. Represents the uncertainty of the state estimate. Updated in place. Stored as a 36-element row-major array.
[in]	q	The previous attitude state as a quaternion [w, x, y, z].
[in]	beta	The previous gyroscope bias estimate vector [3x1] for x, y, z axes.
[in]	omega	The latest gyroscope measurement vector [3x1] in radians/second.
[in]	b_B	The latest magnetometer measurement vector [3x1] in the body frame, in mG.
[in]	height_AGL	The current altitude above ground level in meters (m), used to look up the reference magnetic field from a model.
[out]	q_new	The updated attitude quaternion [w, x, y, z].
[out]	beta_new	The updated gyroscope bias estimate vector [3x1].
[out]	P_new	The updated estimate covariance matrix [6x6].

Returns

None

Definition at line 88 of file mekf.c.

{
  static const double Q[36] = {4.3956900000000333E-11,
                               0.0,
                               0.0,
                               -5.0000000000000008E-23,
                               -0.0,
                               -0.0,
                               0.0,
                               4.3956900000000333E-11,
                               0.0,
                               -0.0,
                               -5.0000000000000008E-23,
                               -0.0,
                               0.0,
                               0.0,
                               4.3956900000000333E-11,
                               -0.0,
                               -0.0,
                               -5.0000000000000008E-23,
                               -5.0000000000000008E-23,
                               -0.0,
                               -0.0,
                               1.0000000000000001E-20,
                               0.0,
                               0.0,
                               -0.0,
                               -5.0000000000000008E-23,
                               -0.0,
                               0.0,
                               1.0000000000000001E-20,
                               0.0,
                               -0.0,
                               -0.0,
                               -5.0000000000000008E-23,
                               0.0,
                               0.0,
                               1.0000000000000001E-20};
  static const double dv[11] = {23950.03, 23938.16, 23926.3,  23914.44,
                                23902.6,  23890.76, 23878.93, 23867.11,
                                23855.29, 23843.49, 23831.69};
  static const double dv1[11] = {2585.64, 2583.71, 2581.78, 2579.85,
                                 2577.92, 2576.0,  2574.08, 2572.15,
                                 2570.24, 2568.32, 2566.4};
  static const double dv2[11] = {39593.08, 39573.32, 39553.57, 39533.84,
                                 39514.11, 39494.41, 39474.71, 39455.03,
                                 39435.36, 39415.7,  39396.06};
  static const double b_y[9] = {0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01};
  static const int R[9] = {HIGHEST_NUMBER, 0, 0, 0, HIGHEST_NUMBER, 0, 0, 0, HIGHEST_NUMBER};
  static const signed char iv[18] = {0, 0, 0, 0, 0, 0, 0, 0, 0,
                                     1, 0, 0, 0, 1, 0, 0, 0, 1};
  static const signed char b[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
  double Phi[36];
  double b_Phi[36];
  double H[18];
  double K[18];
  double b_H[18];
  double cos_term[16];
  double A[9];
  double dv3[9];
  double skew_omega_sq[9];
  double delta_x[6];
  double delta_q[4];
  double axis[3];
  double b_W[3] = {0};
  double A_tmp;
  double a;
  double angle;
  double b_a;
  double d;
  double omega_norm;
  double sin_term;
  double x;
  double y;
  int H_tmp;
  int b_i;
  int cos_term_tmp;
  int i;
  /*  Input parameters: */
  /*  q                 Attitude quaternion [qx,qy,qz,qw] */
  /*  beta              Gyro bias estimate (3x1) */
  /*  P                 Covariance matrix (6x6) */
  /*  omega             Gyroscope measurement (3x1) */
  /*  b_B               Measured magnetic field vector (3x1) */
  /*  height_AGL        Height Above Ground Level */
  /*  Output parameters: */
  /*  q_new         Updated quaternion */
  /*  beta_new      Updated gyro bias estimate */
  /*  P_new         Updated covariance matrix */
  /*  PARAMETERS INITIALIZATION */
  /*  sigma_v       Gyro noise standard deviation */
  /*  sigma_u       Gyro bias random walk noise standard deviation */
  /*  sigma_b       Magnetometer noise standard deviation */
  /*  dt            Time step */
  /* spec-sheet */
  /* placeholder */
  /* spec-sheet */
  /*  MEASUREMENT UPDATE */
  /*  Normalize b_B and b_W */
  y = floor(height_AGL / 1000.0);
  omega_norm = ceil(height_AGL / 1000.0);
  //-----------------------------------------------------------------------------------------------------------------------------------
  //x = (height_AGL / 1000.0 - (y + 1.0)) + 1.0;
 // sin_term = dv[(int)(y + 1.0) - 1];
 // b_W[0] = (dv[(int)(omega_norm + 1.0) - 1] - sin_term) * x + sin_term;
 // sin_term = dv1[(int)(y + 1.0) - 1];
 // b_W[1] = (dv1[(int)(omega_norm + 1.0) - 1] - sin_term) * x + sin_term;
 // sin_term = dv2[(int)(y + 1.0) - 1];
 // b_W[2] = (dv2[(int)(omega_norm + 1.0) - 1] - sin_term) * x + sin_term;
  //---------------------------------------------------------------------------------------------------------------------------------- commented because of the magnetometers question
  /*  Assemble point-rotation matrix for q */
  y = 2.0 * (q[2] * q[2]);
  omega_norm = 2.0 * (q[1] * q[1]);
  A[0] = (1.0 - omega_norm) - y;
  x = q[0] * q[1];
  sin_term = q[2] * q[3];
  A[3] = 2.0 * (x - sin_term);
  A_tmp = q[0] * q[2];
  angle = q[1] * q[3];
  A[6] = 2.0 * (A_tmp + angle);
  A[1] = 2.0 * (x + sin_term);
  x = 1.0 - 2.0 * (q[0] * q[0]);
  A[4] = x - y;
  y = q[1] * q[2];
  sin_term = q[0] * q[3];
  A[7] = 2.0 * (y - sin_term);
  A[2] = 2.0 * (A_tmp - angle);
  A[5] = 2.0 * (y + sin_term);
  A[8] = x - omega_norm;
  /*  Calculate predicted measurement */
  /*  Define sensitivity matrix */
  /*  Create skew-symmetric matrix from a 3x1 vector */
  H[0] = 0.0;
  H[4] = 0.0;
  H[8] = 0.0;
  d = b_W[0];
  A_tmp = b_W[1];
  y = b_W[2];
  for (i = 0; i < 3; i++) {
    H_tmp = 3 * (i + 3);
    H[H_tmp] = 0.0;
    H[H_tmp + 1] = 0.0;
    H[H_tmp + 2] = 0.0;
    axis[i] = (A[i] * d + A[i + 3] * A_tmp) + A[i + 6] * y;
  }
  H[3] = -axis[2];
  H[6] = axis[1];
  H[1] = axis[2];
  H[7] = -axis[0];
  H[2] = -axis[1];
  H[5] = axis[0];
  /*  Calculate Kalman gain */
  for (i = 0; i < 3; i++) {
    for (cos_term_tmp = 0; cos_term_tmp < 6; cos_term_tmp++) {
      d = 0.0;
      for (H_tmp = 0; H_tmp < 6; H_tmp++) {
        d += H[i + 3 * H_tmp] * P[H_tmp + 6 * cos_term_tmp];
      }
      b_H[i + 3 * cos_term_tmp] = d;
    }
    for (cos_term_tmp = 0; cos_term_tmp < 3; cos_term_tmp++) {
      d = 0.0;
      for (H_tmp = 0; H_tmp < 6; H_tmp++) {
        d += b_H[i + 3 * H_tmp] * H[cos_term_tmp + 3 * H_tmp];
      }
      H_tmp = i + 3 * cos_term_tmp;
      skew_omega_sq[H_tmp] = d + (double)R[H_tmp];
    }
  }
  mpower(skew_omega_sq, dv3);
  for (i = 0; i < 6; i++) {
    for (cos_term_tmp = 0; cos_term_tmp < 3; cos_term_tmp++) {
      d = 0.0;
      for (H_tmp = 0; H_tmp < 6; H_tmp++) {
        d += P[i + 6 * H_tmp] * H[cos_term_tmp + 3 * H_tmp];
      }
      b_H[i + 6 * cos_term_tmp] = d;
    }
    d = b_H[i];
    A_tmp = b_H[i + 6];
    y = b_H[i + 12];
    for (cos_term_tmp = 0; cos_term_tmp < 3; cos_term_tmp++) {
      K[i + 6 * cos_term_tmp] =
          (d * dv3[3 * cos_term_tmp] + A_tmp * dv3[3 * cos_term_tmp + 1]) +
          y * dv3[3 * cos_term_tmp + 2];
    }
  }
  /*  Update error state */
  d = b_norm(b_B);
  A_tmp = b_norm(b_W);
  //-----------------------------------------TODO SETTATO A 0 PER VIA DEL TINKERING, EVITARE 0/0
  omega_norm = 0;// b_W[0] / A_tmp;
  x = 0;//b_W[1] / A_tmp;
  y = 0;//b_W[2] / A_tmp;
  //for (i = 0; i < 3; i++) {
    //b_W[i] = b_B[i] / d - ((A[i] * omega_norm + A[i + 3] * x) + A[i + 6] * y);
  //}
  //-----------------------------------------TODO SETTATO A 0 PER VIA DEL TINKERING, EVITARE 0/0
  /*  Update global states */
  /*  QUATERNION UPDATE */
  /*  delta_q quaternion definition */
  /*  Small angle approx */
  /*  Scalar part */
  /*  Multiply delta_q with the current quaternion */
  /*  Normalize the result */
  /*  BIAS UPDATE */
  /*  COVARIANCE MATRIX UPDATE */
  b_eye(Phi);
  for (i = 0; i < 6; i++) {
    d = K[i];
    A_tmp = K[i + 6];
    y = K[i + 12];
    delta_x[i] = (d * b_W[0] + A_tmp * b_W[1]) + y * b_W[2];
    for (cos_term_tmp = 0; cos_term_tmp < 6; cos_term_tmp++) {
      H_tmp = i + 6 * cos_term_tmp;
      b_Phi[H_tmp] =
          Phi[H_tmp] -
          ((d * H[3 * cos_term_tmp] + A_tmp * H[3 * cos_term_tmp + 1]) +
           y * H[3 * cos_term_tmp + 2]);
    }
    for (cos_term_tmp = 0; cos_term_tmp < 6; cos_term_tmp++) {
      d = 0.0;
      for (H_tmp = 0; H_tmp < 6; H_tmp++) {
        d += b_Phi[i + 6 * H_tmp] * P[H_tmp + 6 * cos_term_tmp];
      }
      Phi[i + 6 * cos_term_tmp] = d;
    }
  }
  delta_q[0] = 0.5 * delta_x[0];
  delta_q[1] = 0.5 * delta_x[1];
  delta_q[2] = 0.5 * delta_x[2];
  q_new[0] =
      ((delta_q[0] * q[3] + q[0]) + q[1] * delta_q[2]) - delta_q[1] * q[2];
  q_new[1] =
      ((delta_q[1] * q[3] - q[0] * delta_q[2]) + q[1]) + delta_q[0] * q[2];
  q_new[2] =
      ((delta_q[2] * q[3] + q[0] * delta_q[1]) - delta_q[0] * q[1]) + q[2];
  q_new[3] =
      ((q[3] - q[0] * delta_q[0]) - q[1] * delta_q[1]) - q[2] * delta_q[2];
  d = c_norm(q_new);
  q_new[0] /= d;
  q_new[1] /= d;
  q_new[2] /= d;
  q_new[3] /= d;
  beta_new[0] = beta[0] + delta_x[3];
  beta_new[1] = beta[1] + delta_x[4];
  beta_new[2] = beta[2] + delta_x[5];
  memcpy(&P[0], &Phi[0], 36U * sizeof(double));
  /*  STATE PROPAGATION */
  /*  Calculate the gyroscope measurement without the bias */
  b_W[0] = omega[0] - beta_new[0];
  b_W[1] = omega[1] - beta_new[1];
  b_W[2] = omega[2] - beta_new[2];
  /*  Calculate the magnitude of the angular velocity */
  omega_norm = b_norm(b_W);
  /*  Calculate the angle and axis of rotation */
  angle = omega_norm * 0.01;
  /*  Compute the matrix exponential */
  x = cos(angle);
  sin_term = sin(angle);
  /*  Define the skew symmetric matrix for omega_hat */
  /*  Create skew-symmetric matrix from a 3x1 vector */
  A[0] = 0.0;
  A[3] = -b_W[2];
  A[6] = b_W[1];
  A[1] = b_W[2];
  A[4] = 0.0;
  A[7] = -b_W[0];
  A[2] = -b_W[1];
  A[5] = b_W[0];
  A[8] = 0.0;
  /*  Calculate the squared product of the skew matrix of omega_hat */
  for (b_i = 0; b_i < 3; b_i++) {
    axis[b_i] = b_W[b_i] / omega_norm;
    for (i = 0; i < 3; i++) {
      skew_omega_sq[b_i + 3 * i] =
          (A[b_i] * A[3 * i] + A[b_i + 3] * A[3 * i + 1]) +
          A[b_i + 6] * A[3 * i + 2];
    }
  }
  /*  Calculate the elements of the first row of the phi matrix */
  a = sin_term / omega_norm;
  y = omega_norm * omega_norm;
  b_a = x / y;
  x = (1.0 - x) / y;
  y = (angle - sin_term) / rt_powd_snf(omega_norm, 3.0);
  /*  Assemble phi matrix */
  eye(dv3);
  for (i = 0; i < 3; i++) {
    d = A[3 * i];
    A_tmp = skew_omega_sq[3 * i];
    Phi[6 * i] = (dv3[3 * i] - a * d) + b_a * A_tmp;
    H_tmp = 6 * (i + 3);
    Phi[H_tmp] = (x * d - y * A_tmp) - b_y[3 * i];
    cos_term_tmp = 3 * i + 1;
    d = A[cos_term_tmp];
    A_tmp = skew_omega_sq[cos_term_tmp];
    Phi[6 * i + 1] = (dv3[cos_term_tmp] - a * d) + b_a * A_tmp;
    Phi[H_tmp + 1] = (x * d - y * A_tmp) - b_y[cos_term_tmp];
    cos_term_tmp = 3 * i + 2;
    d = A[cos_term_tmp];
    A_tmp = skew_omega_sq[cos_term_tmp];
    Phi[6 * i + 2] = (dv3[cos_term_tmp] - a * d) + b_a * A_tmp;
    Phi[H_tmp + 2] = (x * d - y * A_tmp) - b_y[cos_term_tmp];
  }
  for (i = 0; i < 6; i++) {
    Phi[6 * i + 3] = iv[3 * i];
    Phi[6 * i + 4] = iv[3 * i + 1];
    Phi[6 * i + 5] = iv[3 * i + 2];
  }
  /*  Calculate the elements of Q_k */
  /*  Assemble Q_k */
  /*  Covariance propagation */
  for (i = 0; i < 6; i++) {
    for (cos_term_tmp = 0; cos_term_tmp < 6; cos_term_tmp++) {
      d = 0.0;
      for (H_tmp = 0; H_tmp < 6; H_tmp++) {
        d += Phi[i + 6 * H_tmp] * P[H_tmp + 6 * cos_term_tmp];
      }
      b_Phi[i + 6 * cos_term_tmp] = d;
    }
    for (cos_term_tmp = 0; cos_term_tmp < 6; cos_term_tmp++) {
      d = 0.0;
      for (H_tmp = 0; H_tmp < 6; H_tmp++) {
        d += b_Phi[i + 6 * H_tmp] * Phi[cos_term_tmp + 6 * H_tmp];
      }
      H_tmp = i + 6 * cos_term_tmp;
      P_new[H_tmp] = d + Q[H_tmp];
    }
  }
  /*  Compute the skew-symmetric matrix for the axis */
  /*  Create skew-symmetric matrix from a 3x1 vector */
  /*  Recompute the sine and cosine terms */
  x = cos(0.5 * angle);
  sin_term = sin(0.5 * angle);
  /*  Construct the Theta matrix */
  /*  Propagate the quaternion */
  skew_omega_sq[0] = sin_term * 0.0;
  skew_omega_sq[3] = sin_term * -axis[2];
  skew_omega_sq[6] = sin_term * axis[1];
  skew_omega_sq[1] = sin_term * axis[2];
  skew_omega_sq[4] = sin_term * 0.0;
  skew_omega_sq[7] = sin_term * -axis[0];
  skew_omega_sq[2] = sin_term * -axis[1];
  skew_omega_sq[5] = sin_term * axis[0];
  skew_omega_sq[8] = sin_term * 0.0;
  for (b_i = 0; b_i < 3; b_i++) {
    d = sin_term * axis[b_i];
    H_tmp = b_i << 2;
    cos_term[H_tmp] = x * (double)b[3 * b_i] - skew_omega_sq[3 * b_i];
    cos_term_tmp = 3 * b_i + 1;
    cos_term[H_tmp + 1] =
        x * (double)b[cos_term_tmp] - skew_omega_sq[cos_term_tmp];
    cos_term_tmp = 3 * b_i + 2;
    cos_term[H_tmp + 2] =
        x * (double)b[cos_term_tmp] - skew_omega_sq[cos_term_tmp];
    cos_term[b_i + 12] = d;
    cos_term[H_tmp + 3] = -d;
  }
  cos_term[15] = x;
  d = q_new[0];
  A_tmp = q_new[1];
  y = q_new[2];
  omega_norm = q_new[3];
  for (i = 0; i < 4; i++) {
    delta_q[i] =
        ((cos_term[i] * d + cos_term[i + 4] * A_tmp) + cos_term[i + 8] * y) +
        cos_term[i + 12] * omega_norm;
  }
  /*  Normalize the propagated quaternion */
  d = c_norm(delta_q);
  q_new[0] = delta_q[0] / d;
  q_new[1] = delta_q[1] / d;
  q_new[2] = delta_q[2] / d;
  q_new[3] = delta_q[3] / d;
}

References b_B, b_eye(), b_norm(), beta, beta_new, c_norm(), eye(), HIGHEST_NUMBER, mpower(), omega, q, Q, q_new, R, and rt_powd_snf().

Here is the call graph for this function: