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

Made some small changes to testSQP to simplify the system and improve convergence.

release/4.3a0
Alex Cunningham 2009-11-18 20:23:07 +00:00
parent 7fc68e2d90
commit d700cd2cac
1 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
cpp/testSQP.cpp
View File

@ -146,7 +146,7 @@ TEST (SQP, problem1_sqp ) {
if (verbose) init.print("Initial State"); if (verbose) init.print("Initial State");
// loop until convergence // loop until convergence
int maxIt = 50; int maxIt = 5;
for (int i = 0; i<maxIt; ++i) { for (int i = 0; i<maxIt; ++i) {
if (verbose) cout << "\n******************************\nIteration: " << i+1 << endl; if (verbose) cout << "\n******************************\nIteration: " << i+1 << endl;
@ -189,7 +189,7 @@ TEST (SQP, problem1_sqp ) {
new GaussianFactor("x", lam*sub(gradG, 0,1, 0,1), // scaled gradG(:,1) new GaussianFactor("x", lam*sub(gradG, 0,1, 0,1), // scaled gradG(:,1)
"y", lam*sub(gradG, 0,1, 1,2), // scaled gradG(:,2) "y", lam*sub(gradG, 0,1, 1,2), // scaled gradG(:,2)
"lam", eye(1), // dlam term "lam", eye(1), // dlam term
Vector_(1.0, lam), // rhs is lambda Vector_(1.0, 0.0), // rhs is zero
1.0)); // arbitrary sigma 1.0)); // arbitrary sigma
// create the actual constraint // create the actual constraint
@ -214,11 +214,17 @@ TEST (SQP, problem1_sqp ) {
if (verbose) delta.print("Delta"); if (verbose) delta.print("Delta");
// update initial estimate // update initial estimate
double gain = 0.3; double gain = 1.0;
// Note:
// It appears that exmap uses adding rather than subtracting to
// update, which breaks the system.
//VectorConfig newState = state.exmap(delta);
VectorConfig newState; VectorConfig newState;
newState.insert("x", state["x"]-gain*delta["x"]); newState.insert("x", state["x"]-gain*delta["x"]);
newState.insert("y", state["y"]-gain*delta["y"]); newState.insert("y", state["y"]-gain*delta["y"]);
newState.insert("lam", state["lam"]-gain*delta["lam"]); newState.insert("lam", state["lam"]-gain*delta["lam"]);
// set the state to the updated state
state = newState; state = newState;
if (verbose) state.print("Updated State"); if (verbose) state.print("Updated State");