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); }
