C++ (Cpp) DOWNLINK_SEND_ALT_KALMAN示例

示例#1

文件： ins_alt_float.c 项目： HolyFox812/paparazzi

void alt_kalman(float z_meas) {
  float DT;
  float R;
  float SIGMA2;

#if USE_BAROMETER
#if USE_BARO_MS5534A
  if (alt_baro_enabled) {
    DT = BARO_DT;
    R = baro_MS5534A_r;
    SIGMA2 = baro_MS5534A_sigma2;
  } else
#elif USE_BARO_ETS
  if (baro_ets_enabled) {
    DT = BARO_ETS_DT;
    R = baro_ets_r;
    SIGMA2 = baro_ets_sigma2;
  } else
#elif USE_BARO_MS5611
  if (baro_ms5611_enabled) {
    DT = BARO_MS5611_DT;
    R = baro_ms5611_r;
    SIGMA2 = baro_ms5611_sigma2;
  } else
#elif USE_BARO_AMSYS
  if (baro_amsys_enabled) {
    DT = BARO_AMSYS_DT;
    R = baro_amsys_r;
    SIGMA2 = baro_amsys_sigma2;
  } else
#elif USE_BARO_BMP
  if (baro_bmp_enabled) {
    DT = BARO_BMP_DT;
    R = baro_bmp_r;
    SIGMA2 = baro_bmp_sigma2;
  } else
#endif
#endif // USE_BAROMETER
  {
    DT = GPS_DT;
    R = GPS_R;
    SIGMA2 = GPS_SIGMA2;
  }

  float q[2][2];
  q[0][0] = DT*DT*DT*DT/4.;
  q[0][1] = DT*DT*DT/2.;
  q[1][0] = DT*DT*DT/2.;
  q[1][1] = DT*DT;


  /* predict */
  ins_alt += ins_alt_dot * DT;
  p[0][0] = p[0][0]+p[1][0]*DT+DT*(p[0][1]+p[1][1]*DT) + SIGMA2*q[0][0];
  p[0][1] = p[0][1]+p[1][1]*DT + SIGMA2*q[0][1];
  p[1][0] = p[1][0]+p[1][1]*DT + SIGMA2*q[1][0];
  p[1][1] = p[1][1] + SIGMA2*q[1][1];

  /* error estimate */
  float e = p[0][0] + R;

  if (fabs(e) > 1e-5) {
    float k_0 = p[0][0] / e;
    float k_1 =  p[1][0] / e;
    e = z_meas - ins_alt;

    /* correction */
    ins_alt += k_0 * e;
    ins_alt_dot += k_1 * e;

    p[1][0] = -p[0][0]*k_1+p[1][0];
    p[1][1] = -p[0][1]*k_1+p[1][1];
    p[0][0] = p[0][0] * (1-k_0);
    p[0][1] = p[0][1] * (1-k_0);
  }

#ifdef DEBUG_ALT_KALMAN
  DOWNLINK_SEND_ALT_KALMAN(DefaultChannel,DefaultDevice,&(p[0][0]),&(p[0][1]),&(p[1][0]), &(p[1][1]));
#endif
}

示例#2

文件： estimator.c 项目： Bruzzlee/paparazzi

void alt_kalman(float gps_z) {
  float DT;
  float R;
  float SIGMA2;

#ifdef USE_BARO_MS5534A
  if (alt_baro_enabled) {
    DT = BARO_DT;
    R = baro_MS5534A_r;
    SIGMA2 = baro_MS5534A_sigma2;
  } else
#elif defined(USE_BARO_ETS)
  if (baro_ets_enabled) {
    DT = BARO_ETS_DT;
    R = baro_ets_r;
    SIGMA2 = baro_ets_sigma2;
  } else
#endif
  {
    DT = GPS_DT;
    R = GPS_R;
    SIGMA2 = GPS_SIGMA2;
  }

  float q[2][2];
  q[0][0] = DT*DT*DT*DT/4.;
  q[0][1] = DT*DT*DT/2.;
  q[1][0] = DT*DT*DT/2.;
  q[1][1] = DT*DT;


  /* predict */
  estimator_z += estimator_z_dot * DT;
  p[0][0] = p[0][0]+p[1][0]*DT+DT*(p[0][1]+p[1][1]*DT) + SIGMA2*q[0][0];
  p[0][1] = p[0][1]+p[1][1]*DT + SIGMA2*q[0][1];
  p[1][0] = p[1][0]+p[1][1]*DT + SIGMA2*q[1][0];
  p[1][1] = p[1][1] + SIGMA2*q[1][1];

  /* error estimate */
  float e = p[0][0] + R;

  if (fabs(e) > 1e-5) {
    float k_0 = p[0][0] / e;
    float k_1 =  p[1][0] / e;
    e = gps_z - estimator_z;

    /* correction */
    estimator_z += k_0 * e;
    estimator_z_dot += k_1 * e;

    p[1][0] = -p[0][0]*k_1+p[1][0];
    p[1][1] = -p[0][1]*k_1+p[1][1];
    p[0][0] = p[0][0] * (1-k_0);
    p[0][1] = p[0][1] * (1-k_0);
  }

#ifdef DEBUG_ALT_KALMAN
  DOWNLINK_SEND_ALT_KALMAN(&(p[0][0]),&(p[0][1]),&(p[1][0]), &(p[1][1]));
#endif

}