double get(int_8 i) { if (i>=0 && i<data.size()) return data[i]; else { cout << "Var-get: out of bounds" << endl; exit(1); } }
double* operator[](int_8 const &index) { if (index>=0 && index<data.size()) return &data[index]; else { cout << "Var: out of bounds " << endl; exit(1); } }
int KalmanFilter::init(VecX mu, MatX R, MatX C, MatX Q) { int nb_states; nb_states = mu.size(); this->initialized = true; this->mu = mu; this->B = MatX::Zero(nb_states, nb_states); this->R = R; this->C = C; this->Q = Q; this->S = MatX::Identity(nb_states, nb_states); this->I = MatX::Identity(nb_states, nb_states); this->K = MatX::Zero(nb_states, nb_states); this->mu_p = VecX::Zero(nb_states); this->S_p = MatX::Zero(nb_states, nb_states); return 0; }
void set(double a) { data.data.assign(data.size(), a); }