12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix easy Kalman filter example

release/4.3a0
Chris Beall 2013-08-19 21:32:51 +00:00
parent 5b1fd32871
commit 46ab10fb06
2 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
examples/CMakeLists.txt
View File

@ -9,7 +9,6 @@ set (excluded_examples #"")
"${CMAKE_CURRENT_SOURCE_DIR}/DiscreteBayesNet_FG.cpp" "${CMAKE_CURRENT_SOURCE_DIR}/DiscreteBayesNet_FG.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/UGM_chain.cpp" "${CMAKE_CURRENT_SOURCE_DIR}/UGM_chain.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/UGM_small.cpp" "${CMAKE_CURRENT_SOURCE_DIR}/UGM_small.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/easyPoint2KalmanFilter.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/elaboratePoint2KalmanFilter.cpp" "${CMAKE_CURRENT_SOURCE_DIR}/elaboratePoint2KalmanFilter.cpp"
) )

11
examples/easyPoint2KalmanFilter.cpp
View File

@ -39,10 +39,11 @@ int main() {
Point2 x_initial(0.0, 0.0); Point2 x_initial(0.0, 0.0);
SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
// Create Key for initial pose
Symbol x0('x',0);
// Create an ExtendedKalmanFilter object // Create an ExtendedKalmanFilter object
ExtendedKalmanFilter<Point2> ekf(x_initial, P_initial); ExtendedKalmanFilter<Point2> ekf(x0, x_initial, P_initial);
// Now predict the state at t=1, i.e. argmax_{x1} P(x1) = P(x1|x0) P(x0) // Now predict the state at t=1, i.e. argmax_{x1} P(x1) = P(x1|x0) P(x0)
// In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t} // In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t}
@ -60,8 +61,8 @@ int main() {
SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1), true); SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1), true);
// This simple motion can be modeled with a BetweenFactor // This simple motion can be modeled with a BetweenFactor
// Create Keys // Create Key for next pose
Symbol x0('x',0), x1('x',1); Symbol x1('x',1);
// Predict delta based on controls // Predict delta based on controls
Point2 difference(1,0); Point2 difference(1,0);
// Create Factor // Create Factor