kalmanFilter.c

Go to the documentation of this file.

/*
 * Academic License - for use in teaching, academic research, and meeting
 * course requirements at degree granting institutions only.  Not for
 * government, commercial, or other organizational use.
 * File: kalmanFilter_VES_definitive.c
 *
 * MATLAB Coder version            : 24.2
 * C/C++ source code generated on  : 05-May-2025 19:53:20
 */
 
/* Include Files */
#include "kalmanFilter.h"
#include "mrdivide_helper.h"
#include <math.h>
 
 
 
/* Function Definitions */
/*
 * Implements a Kalman filter to estimate state variables (barometric altitude,
 * GNSS velocity) with conditional measurement updates based on GNSS signal
 * availability.
 *
 *  Parameters:
 *      x_est (2x1 array): Current state estimate [barometric_altitude;
 * gnss_velocity] P (2x2 array): Current estimate covariance matrix z (2x1
 * array): Measurement vector [measured_barometric_altitude;
 * measured_gnss_velocity] alpha (float): Current tilt angle (in radians) of the
 * rocket a_m (float): Measured acceleration (affects the system's state
 * dynamics) Ts (float): Time step Q (2x2 array): Process noise covariance
 * matrix R (2x2 array): Measurement noise covariance matrix gnss_signal
 * (logical): Flag indicating GNSS signal availability (1=available,
 * 0=unavailable)
 *
 *  Returns:
 *      tuple: Updated state estimate `x_est` and covariance matrix `P`
 *
 * Arguments    : double x_est[2]
 *                double P[4]
 *                const double z[2]
 *                double alpha
 *                double a_m
 *                double Ts
 *                const double Q[4]
 *                const double R[4]
 *                double gnss_signal
 * Return Type  : void
 */
void kalmanFilter(double x_est[2], double P[4],
                                 const double z[2], double alpha, double a_m,
                                 double Ts, const double Q[4],
                                 const double R[4], bool gnss_signal)
{
  static const signed char b_a[4] = {1, 0, 0, 1};
  double F[4];
  double P0[4];
  double x_pred[2];
  double cos_alpha;
  double d;
  double d1;
  double d2;
  double d3;
  double d4;
  int a_tmp;
  int i;
  /*  Compute cos_alpha from the input tilt angle alpha */
  cos_alpha = cos(alpha);
  /*  State transition matrix */
  F[0] = 1.0;
  F[2] = Ts * cos_alpha;
  F[1] = 0.0;
  F[3] = 1.0;
  /*  Barometric altitude depends on vertical velocity component */
  /*  GNSS velocity has simple integration relationship */
  /*  Control input matrix */
  /*  Barometric altitude change */
  /*  GNSS velocity change */
  /*  Prediction step */
  x_pred[0] =
      (x_est[0] + x_est[1] * F[2]) + Ts * Ts / 2.0 * (a_m * cos_alpha - 9.81);
  x_pred[1] = (0.0 * x_est[0] + x_est[1]) + Ts * (a_m - 9.81 * cos_alpha);
  /*  Observation matrix - identity when both measurements available */
  /*  Always measure barometric altitude */
  /*  GNSS velocity measurement depends on signal */
  /*  Modify measurement update based on GNSS signal availability */
  if (gnss_signal == true) {
    double K[4];
    double a[4];
    double b_Ts[2];
    double P0_idx_0;
    double P0_idx_1;
    double P0_idx_2;
    double P0_idx_3;
    int b_a_tmp;
    /*  Full update (both altitude and velocity) */
    for (i = 0; i < 2; i++) {
      a_tmp = b_a[i];
      b_a_tmp = b_a[i + 2];
      cos_alpha = (double)a_tmp * P[0] + (double)b_a_tmp * P[1];
      d = (double)a_tmp * P[2] + (double)b_a_tmp * P[3];
      P0[i] = (cos_alpha + d * 0.0) + R[i];
      P0[i + 2] = (cos_alpha * 0.0 + d) + R[i + 2];
      cos_alpha = P[i + 2];
      a[i] = P[i] + cos_alpha * 0.0;
      a[i + 2] = P[i] * 0.0 + cos_alpha;
    }
    cos_alpha = P0[0] * P0[3] - P0[1] * P0[2];
    P0_idx_0 = P0[3] / cos_alpha;
    P0_idx_2 = -P0[2] / cos_alpha;
    P0_idx_1 = -P0[1] / cos_alpha;
    P0_idx_3 = P0[0] / cos_alpha;
    cos_alpha = P[0];
    d = P[1];
    d1 = P[2];
    d2 = P[3];
    d3 = x_pred[0];
    d4 = x_pred[1];
    for (i = 0; i < 2; i++) {
      double d5;
      double d6;
      double d7;
      d5 = a[i + 2];
      d6 = a[i];
      d7 = d6 * P0_idx_0 + d5 * P0_idx_1;
      K[i] = d7;
      d5 = d6 * P0_idx_2 + d5 * P0_idx_3;
      K[i + 2] = d5;
      a_tmp = b_a[i];
      d6 = (double)a_tmp - (d7 + d5 * 0.0);
      a[i] = d6;
      b_a_tmp = b_a[i + 2];
      d5 = (double)b_a_tmp - (d7 * 0.0 + d5);
      a[i + 2] = d5;
      P0[i] = d6 * cos_alpha + d5 * d;
      P0[i + 2] = d6 * d1 + d5 * d2;
      b_Ts[i] = z[i] - ((double)a_tmp * d3 + (double)b_a_tmp * d4);
    }
    x_est[0] = x_pred[0] + (K[0] * b_Ts[0] + b_Ts[1] * K[2]);
    x_est[1] = x_pred[1] + (b_Ts[0] * K[1] + b_Ts[1] * K[3]);
  } else {
    double a[4];
    /*  Partial update (only altitude) */
    /*  Only observe altitude */
    /*  Only altitude measurement noise */
    x_est[0] = P[0] + P[2] * 0.0;
    x_est[1] = P[1] + P[3] * 0.0;
    cos_alpha =
        1.0 / (((P[0] + 0.0 * P[1]) + (P[2] + 0.0 * P[3]) * 0.0) + R[0]);
    x_est[0] *= cos_alpha;
    x_est[1] *= cos_alpha;
    a[0] = 1.0 - x_est[0];
    a[1] = 0.0 - x_est[1];
    a[2] = 0.0 - x_est[0] * 0.0;
    a[3] = 1.0 - x_est[1] * 0.0;
    /*  Only update altitude (first element of state vector) */
    cos_alpha = 0.0;
    d = P[0];
    d1 = P[1];
    d2 = P[2];
    d3 = P[3];
    for (i = 0; i < 2; i++) {
      double d5;
      d4 = a[i + 2];
      d5 = a[i];
      P0[i] = d5 * d + d4 * d1;
      P0[i + 2] = d5 * d2 + d4 * d3;
      cos_alpha += (1.0 - (double)i) * x_pred[i];
    }
    cos_alpha = z[0] - cos_alpha;
    x_est[0] = x_pred[0] + x_est[0] * cos_alpha;
    x_est[1] = x_pred[1] + x_est[1] * cos_alpha;
  }
  /*  Always update covariance matrix */
  cos_alpha = P0[0];
  d = P0[1];
  d1 = P0[2];
  d2 = P0[3];
  for (i = 0; i < 2; i++) {
    d3 = F[i + 2];
    a_tmp = (int)F[i];
    d4 = (double)a_tmp * cos_alpha + d3 * d;
    d3 = (double)a_tmp * d1 + d3 * d2;
    P[i] = (d4 + d3 * F[2]) + Q[i];
    P[i + 2] = (d4 * 0.0 + d3) + Q[i + 2];
  }
}
 
 
/*
 * File trailer for kalmanFilter_VES_definitive.c
 *
 * [EOF]
 */