void test_EKF_interface_simulated_data()
{
vcl_string panFile("/Users/jimmy/Data/pan_regression/real_data/RF_raw_output.txt");
vcl_string velocityFile("/Users/jimmy/Data/pan_regression/real_data/RF_raw_velocity.txt");
vcl_string panGdFile("/Users/jimmy/Data/pan_regression/real_data/quarter_2_spherical_map/testing_fn_pan_features.txt");
vnl_matrix<double> panMat;
vnl_matrix<double> velocityMat;
vnl_matrix<double> panGdMat;
VnlPlus::readMat(panFile.c_str(), panMat);
VnlPlus::readMat(velocityFile.c_str(), velocityMat);
VnlPlus::readMat(panGdFile.c_str(), panGdMat);
vcl_vector<double> observed_pan;
vcl_vector<double> observed_velocity;
for (int i = 0; i<panMat.rows(); i++) {
observed_pan.push_back(panMat(i, 1));
observed_velocity.push_back(velocityMat(i, 1));
}
vcl_vector<double> smoothed_pan;
vcl_vector<double> smoothed_velocity;
vnl_vector<double> x0(2, 0);
x0[0] = 12; x0[1] = 0.0;
vnl_matrix<double> P0(2, 2, 0);
P0(0, 0) = 0.1; P0(1, 1) = 0.1;
vnl_matrix<double> R(2, 2, 0);
R(0, 0) = 0.06; R(1, 1) = 0.01;
vnl_matrix<double> Q(2, 2, 0);
Q(0, 0) = 0.00000004; Q(1, 1) = 0.00000001;
cameraPlaningPan_EKF aEKF;
aEKF.init(x0, P0);
aEKF.initNoiseCovariance(Q, R);
for (int i = 0; i<observed_pan.size(); i++) {
vnl_vector<double> zk(2, 0);
zk[0] = observed_pan[i];
zk[1] = observed_velocity[i];
vnl_vector<double> xk;
vnl_matrix<double> pk;
aEKF.update(zk, xk, pk);
smoothed_pan.push_back(xk[0]);
smoothed_velocity.push_back(xk[1]);
}
vnl_vector<double> smoothedPanVec(&smoothed_pan[0], (int)smoothed_pan.size());
vnl_vector<double> smoothedVelocityVec(&smoothed_velocity[0], (int)smoothed_velocity.size());
vcl_string save_file("EKF_observed_pan_velocity_26.mat");
vnl_matlab_filewrite awriter(save_file.c_str());
awriter.write(smoothedPanVec, "sm_pan");
awriter.write(panMat.get_column(1), "pan");
awriter.write(panGdMat.get_column(1), "gdPan");
awriter.write(velocityMat.get_column(1), "velo");
awriter.write(smoothedVelocityVec, "sm_velo");
printf("save to %s\n", save_file.c_str());
}
void MDAL::DriverFlo2D::parseVELFPVELOCFile( const std::string &datFileName )
{
// these files are optional, so if not present, reading is skipped
size_t nVertices = mMesh->verticesCount();
std::vector<double> maxVel( nVertices );
{
std::string velocityFile( fileNameFromDir( datFileName, "VELFP.OUT" ) );
if ( !MDAL::fileExists( velocityFile ) )
{
return; //optional file
}
std::ifstream velocityStream( velocityFile, std::ifstream::in );
std::string line;
size_t vertex_idx = 0;
// VELFP.OUT - COORDINATES (ELEM NUM, X, Y, MAX VEL) - Maximum floodplain flow velocity;
while ( std::getline( velocityStream, line ) )
{
if ( vertex_idx == nVertices ) throw MDAL_Status::Err_IncompatibleMesh;
line = MDAL::rtrim( line );
std::vector<std::string> lineParts = MDAL::split( line, ' ' );
if ( lineParts.size() != 4 )
{
throw MDAL_Status::Err_UnknownFormat;
}
double val = getDouble( lineParts[3] );
maxVel[vertex_idx] = val;
vertex_idx++;
}
}
{
std::string velocityFile( fileNameFromDir( datFileName, "VELOC.OUT" ) );
if ( !MDAL::fileExists( velocityFile ) )
{
return; //optional file
}
std::ifstream velocityStream( velocityFile, std::ifstream::in );
std::string line;
size_t vertex_idx = 0;
// VELOC.OUT - COORDINATES (ELEM NUM, X, Y, MAX VEL) - Maximum channel flow velocity
while ( std::getline( velocityStream, line ) )
{
if ( vertex_idx == nVertices ) throw MDAL_Status::Err_IncompatibleMesh;
line = MDAL::rtrim( line );
std::vector<std::string> lineParts = MDAL::split( line, ' ' );
if ( lineParts.size() != 4 )
{
throw MDAL_Status::Err_UnknownFormat;
}
double val = getDouble( lineParts[3] );
if ( !is_nodata( val ) ) // overwrite value from VELFP if it is not 0
{
maxVel[vertex_idx] = val;
}
vertex_idx++;
}
}
addStaticDataset( true, maxVel, "Velocity/Maximums", datFileName );
}
