int main(int argc, char **argv)
{
ros::init(argc, argv, "ukf_navigation_node");
ros::NodeHandle nhLocal("~");
std::vector<double> args(3, 0);
nhLocal.param("alpha", args[0], 0.001);
nhLocal.param("kappa", args[1], 0.0);
nhLocal.param("beta", args[2], 2.0);
RobotLocalization::RosUkf ukf(args);
ukf.run();
return 0;
}
void
runTest(void)
{
double t=0;
double dt=0.01;
int N=400;
std::vector<double> positions;
std::vector<double> velocities;
positions.resize(N);
velocities.resize(N);
std::generate_n(positions.begin(),
N,
[&t, &dt](void)
{
double x=sin(t*2*M_PI);
t += dt;
return x;
});
t = 0.0;
std::generate_n(velocities.begin(),
N,
[&t, &dt](void)
{
double x=2*M_PI*cos(t*2*M_PI);
t += dt;
return x;
});
std::vector<double> noisy_positions;
noisy_positions.resize(N);
std::mt19937 generator;
std::normal_distribution<double> normal(0.0, 0.1);
std::transform(positions.begin(),
positions.end(),
noisy_positions.begin(),
[&normal, &generator](double x)
{
return x+normal(generator);
});
TestUKF ukf(1e-3, 2.0, 0.0);
// acceleration noise
double sigma = (2*M_PI)*(2*M_PI)*10;
TestState st;
st.v=0.0;
ukf.reset(st);
Eigen::MatrixXd measurement_noise(1,1);
measurement_noise(0,0) = 0.02;
std::vector<Eigen::MatrixXd> mcovs;
mcovs.push_back(measurement_noise);
t = 0.0;
std::cout << t << ", " <<
positions[0] << ", " <<
velocities[0] << ", " <<
ukf.state().x << ", " <<
sqrt(ukf.state().Covariance(0,0)) << ", " <<
ukf.state().v << ", " <<
sqrt(ukf.state().Covariance(1,1)) << ", " <<
0.0 << std::endl;
int correct_every=1;
for(int i=0;i<N;i++)
{
ukf.predict(dt, process_noise(dt, sigma));
t += dt;
PositionMeasurement m;
m.x = noisy_positions[i];
if(i%correct_every==0)
ukf.correct(m, mcovs);
std::cout << t << ", " <<
positions[i] << ", " <<
velocities[i] << ", " <<
ukf.state().x << ", " <<
sqrt(ukf.state().Covariance(0,0)) << ", " <<
ukf.state().v << ", " <<
sqrt(ukf.state().Covariance(1,1)) << ", " <<
m.x << ", " <<
sqrt(measurement_noise(0,0)) <<
std::endl;
}
}
