#include <Base/Math/Random.hh>
#include <StateEstimator/Kalman.hh>
using namespace BIAS;
using namespace std;
const double meas_noise = 2.0;
const double process_noise = 1.0;
{
res = A * state;
const int dim = res.
Size();
for (int i=0; i<dim; i++){
noise[i] =
}
return res + noise;
}
{
const int meas_dim = meas.
Size();
for (int r=0; r<meas_dim; r++){
meas_cov[r][r] = std_dev * std_dev;
meas[r] = gt_meas[r] +
}
}
int main(int , char ** )
{
const int state_dim = 2;
const int meas_dim = 2;
state(state_dim);
meas_cov(meas_dim, meas_dim),
update_cov(state_dim, state_dim),
process_covariance(state_dim, state_dim);
double std_dev;
for (int r=0; r<state_dim; r++){
process_covariance[r][r] = std_dev * std_dev;
}
pred_cov.SetZero();
for (int r=0; r<state_dim; r++){
pred_cov[r][r] = std_dev * std_dev;
pred[r] = state[r] +
}
cout << "# ground truth system state: " << state
<< "\n# intial system state guess: " << pred
<< "\n# covariance matrix of initial system state guess: "
<<pred_cov<<endl;
kafi.
Init(A, H, pred, pred_cov);
const int num_cycles=100;
cout <<"# time\tstate\t\tpred\t\tpred_cov\tmeas\t\tmeas-cov\tup\t\tup-cov\n";
for (int i=0; i<num_cycles; i++){
state = Process(A, state, process_covariance);
GenerateNoisyMeasurements(state, H, meas, meas_cov);
cout <<i;
for (int d=0; d<state_dim; d++){
cout <<"\t"<<state[d] << "\t" << pred[d]
<<"\t"<<sqrt(pred_cov[d][d])<< "\t";
if (d<meas_dim){
cout << meas[d] << "\t" << sqrt(meas_cov[d][d]) << "\t" ;
} else {
cout << "nan" << "\t" << "nan" << "\t";
}
cout << update[d] << "\t"
<< sqrt(update_cov[d][d]);
}
cout << endl;
}
return 0;
}