Exemple #1
0
  void KFilter<T, OQ, OVR, DBG>::makeProcess() {

    // x = Ax + Bu + Ww    n.1 = n.n * n.1 + n.nu * nu.1
    makeB();

    K_UINT_32 i, j;
    x__.resize(EKFilter<T, OQ, OVR, DBG>::n);

    for (i = 0; i < EKFilter<T, OQ, OVR, DBG>::n; ++i) {

      x__(i) = T(0.0);

      for (j = 0; j < EKFilter<T, OQ, OVR, DBG>::n; ++j)
        x__(i) += EKFilter<T, OQ, OVR, DBG>::A(i,j) * EKFilter<T, OQ, OVR, DBG>::x(j);

      for (j = 0; j < EKFilter<T, OQ, OVR, DBG>::nu; ++j)
        x__(i) += B(i,j) * EKFilter<T, OQ, OVR, DBG>::u(j);

    }

    EKFilter<T, OQ, OVR, DBG>::x.swap(x__);

  }
Exemple #2
0
  void KFilter<T, BEG, OQ, OVR, DBG>::makeProcess() {

    // x = Ax + Bu + Ww    n.1 = n.n * n.1 + n.nu * nu.1
    makeB();

    K_UINT_32 i, j;
    x__.resize(this->n);

    for (i = BEG; i < this->n + BEG; ++i) {

      x__(i) = T(0.0);

      for (j = BEG; j < this->n + BEG; ++j)
        x__(i) += this->A(i,j) * this->x(j);

      for (j = BEG; j < this->nu + BEG; ++j)
        x__(i) += B(i,j) * this->u(j);

    }

    this->x.swap(x__);

  }