Strange Behaviour when using VARIABLE WEIGHTING MATRIX

[Max B]

**NEVERMIND, found my error, can be deleted!
**

Hi,
I'm using the code generation tool for an MPC and it works fine when I use dynamically changeable weights as mentioned here.
My model is the following:

DifferentialState x1;
  DifferentialState x2;
  DifferentialState theta;
  DifferentialState v1;
  DifferentialState v2;
  DifferentialState omega;

  Control a1;
  Control a2;
  Control atheta;

  // Model equations
  DifferentialEquation f;
  f << dot(x1) == v1  cos(theta) - v2  sin(theta);
  f << dot(x2) == v1  sin(theta) + v2  cos(theta);
  f << dot(theta) == omega;
  f << dot(v1) == a1;
  f << dot(v2) == a2;
  f << dot(omega) == atheta;

  // Reference function and weighting matrices
  Function h, hN;
  h << x1 << x2 << theta << v1 << v2 << omega;
  hN << x1 << x2 << theta << v1 << v2 << omega;

  BMatrix Q = eye<bool>(h.getDim());
  BMatrix QN = eye<bool>(hN.getDim());

Unfortunately when I use mpc.set(CGUSEVARIABLEWEIGHTINGMATRIX, YES); and when I set the weights dynamically like that

for (unsigned row = 0; row < N  dimension; ++row)
  {
    for (unsigned column = 0; column < dimension; ++column)
    {
      if (row % dimension == column)
      {
        acadoVariables.W[row  dimension + column] = 1.;
        acadoVariables.WN[row % N  dimension + column] = 0.01;
      }
      else
      {
        acadoVariables.W[row  dimension + column] = 1;
        acadoVariables.WN[row % N  dimension + column] = 0.05;
      }
    }

*which results in a nearly identity matrix for every prediction step, all the predicted controls are set to zero, all the "velocities" are set to the off diagonal elements of the WN Matrix (0.05) and the "positions" are set to the same value in the first step and change according to the "predicted" velocities afterwards.
These are the only changes to the working version in which the weights are set dynamically but are the same for all the prediction steps.

Does anybody know what I'm doing wrong or could anybody show me a working version with mpc.set(CGUSEVARIABLEWEIGHTINGMATRIX, YES) and C++ as I've only found the example with Matlab.

Thank you so much in advance!*

[Max B]

I modified the getting started example in a similar way as my code and it seems to work fine. So probably I have to search my good on my own :(

Generating Code

/*

 *    This file is part of ACADO Toolkit.
 *
 *    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
 *    Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
 *    Milan Vukov, Rien Quirynen, KU Leuven.
 *    Developed within the Optimization in Engineering Center (OPTEC)
 *    under supervision of Moritz Diehl. All rights reserved.
 *
 *    ACADO Toolkit is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License as published by the Free Software Foundation; either
 *    version 3 of the License, or (at your option) any later version.
 *
 *    ACADO Toolkit is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with ACADO Toolkit; if not, write to the Free Software
 *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 *
 */

/**

*    \file   examples/code_generation/getting_started.cpp
*    \author Hans Joachim Ferreau, Boris Houska
*    \date   2011-2013
*/

#include <acado_code_generation.hpp>

int main()
{
  USING_NAMESPACE_ACADO

  // Variables:
  DifferentialState p;      // the trolley position
  DifferentialState v;      // the trolley velocity
  DifferentialState phi;    // the excitation angle
  DifferentialState omega;  // the angular velocity
  Control a;                // the acc. of the trolley

  const double g = 9.81;  // the gravitational constant
  const double b = 0.20;  // the friction coefficient

  // Model equations:
  DifferentialEquation f;

  f << dot(p) == v;
  f << dot(v) == a;
  f << dot(phi) == omega;
  f << dot(omega) == -g * sin(phi) - a * cos(phi) - b * omega;

  // Reference functions and weighting matrices:
  Function h, hN;
  h << p << v << phi << omega << a;
  hN << p << v << phi << omega;

  // Provide defined weighting matrices:
  BMatrix W = eye<bool>(h.getDim());
  BMatrix WN = eye<bool>(hN.getDim());

  // Or provide sparsity patterns for the weighting matrices
  //    BMatrix W = eye<bool>( h.getDim() );
  //    BMatrix WN = eye<bool>( hN.getDim() );

  //
  // Optimal Control Problem
  //
  OCP ocp(0.0, 3.0, 10);

  ocp.subjectTo(f);

  ocp.minimizeLSQ(W, h);
  ocp.minimizeLSQEndTerm(WN, hN);

  ocp.subjectTo(-1.0 <= a <= 1.0);
  ocp.subjectTo(-0.5 <= v <= 1.5);

  // Export the code:
  OCPexport mpc(ocp);

  mpc.set(HESSIAN_APPROXIMATION, GAUSS_NEWTON);
  mpc.set(DISCRETIZATION_TYPE, SINGLE_SHOOTING);
  mpc.set(INTEGRATOR_TYPE, INT_RK4);
  mpc.set(NUM_INTEGRATOR_STEPS, 30);

  mpc.set(QP_SOLVER, QP_QPOASES);
  //    mpc.set( HOTSTART_QP,                 YES             );
  //    mpc.set( LEVENBERG_MARQUARDT,         1.0e-4          );
  mpc.set(GENERATE_TEST_FILE, NO);
  mpc.set(GENERATE_MAKE_FILE, NO);
  mpc.set(GENERATE_MATLAB_INTERFACE, YES);
  mpc.set(GENERATE_SIMULINK_INTERFACE, YES);
  mpc.set(CG_USE_VARIABLE_WEIGHTING_MATRIX, YES);

  //    mpc.set( USE_SINGLE_PRECISION,        YES             );

  if (mpc.exportCode("getting_started_export") != SUCCESSFUL_RETURN)
    exit(EXIT_FAILURE);

  mpc.printDimensionsQP();

  return EXIT_SUCCESS;
}

test.c

/*

 *    This file was auto-generated using the ACADO Toolkit.
 *
 *    While ACADO Toolkit is free software released under the terms of
 *    the GNU Lesser General Public License (LGPL), the generated code
 *    as such remains the property of the user who used ACADO Toolkit
 *    to generate this code. In particular, user dependent data of the code
 *    do not inherit the GNU LGPL license. On the other hand, parts of the
 *    generated code that are a direct copy of source code from the
 *    ACADO Toolkit or the software tools it is based on, remain, as derived
 *    work, automatically covered by the LGPL license.
 *
 *    ACADO Toolkit is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 *
 */

/*

IMPORTANT: This file should serve as a starting point to develop the user
code for the OCP solver. The code below is for illustration purposes. Most
likely you will not get good results if you execute this code without any
modification(s).

Please read the examples in order to understand how to write user code how
to run the OCP solver. You can find more info on the website:
www.acadotoolkit.org

*/

#include "acado_common.h"
#include "acado_auxiliary_functions.h"

#include <stdio.h>

/* Some convenient definitions. */
#define NX ACADO_NX   /* Number of differential state variables.  */
#define NXA ACADO_NXA /* Number of algebraic variables. */
#define NU ACADO_NU   /* Number of control inputs. */
#define NOD ACADO_NOD /* Number of online data values. */

#define NY ACADO_NY   /* Number of measurements/references on nodes 0..N - 1. */
#define NYN ACADO_NYN /* Number of measurements/references on node N. */

#define N ACADO_N /* Number of intervals in the horizon. */

#define NUM_STEPS 10 /* Number of real-time iterations. */
#define VERBOSE 1    /* Show iterations: 1, silent: 0.  */

/* Global variables used by the solver. */
ACADOvariables acadoVariables;
ACADOworkspace acadoWorkspace;

/* A template for testing of the solver. */
int main()
{
  /* Some temporary variables. */
  int i, j, k, iter;
  acado_timer t;

  /* Initialize the solver. */
  acado_initializeSolver();

  /* Initialize the states and controls. */
  for (i = 0; i < NX * (N + 1); ++i)
    acadoVariables.x[i] = 0.0;
  for (i = 0; i < NU * N; ++i)
    acadoVariables.u[i] = 0.0;

  /* Initialize the measurements/reference. */
  for (i = 0; i < NY * N; ++i)
    acadoVariables.y[i] = 0.0;
  for (i = 0; i < NYN; ++i)
    acadoVariables.yN[i] = 0.0;

/* MPC: initialize the current state feedback. */
#if ACADO_INITIAL_STATE_FIXED
  for (i = 0; i < NX; ++i)
    acadoVariables.x0[i] = 0.1;
#endif

  /* Initialize Weights */
  for (i = 0; i < N * NY; i++)
  {
    for (j = 0; j < NY; j++)
    {
      if (i % NY == j)
      {
        acadoVariables.W[i * NY + j] = 1;
      }
    }
  }

  for (i = 0; i < NYN; i++)
  {
    acadoVariables.WN[i + NYN * i] = 1;
  }

  if (VERBOSE)
    acado_printHeader();

  /* Prepare first step */
  acado_preparationStep();

  /* Get the time before start of the loop. */
  acado_tic(&t);

  /* The "real-time iterations" loop. */
  for (iter = 0; iter < NUM_STEPS; ++iter)
  {
    /* Perform the feedback step. */
    acado_feedbackStep();

    /* Apply the new control immediately to the process, first NU components. */

    if (VERBOSE)
      printf("\tReal-Time Iteration %d:  KKT Tolerance = %.3e\n\n", iter, acado_getKKT());

    /* Optional: shift the initialization (look at acado_common.h). */
    /* acado_shiftStates(2, 0, 0); */
    /* acado_shiftControls( 0 ); */

    /* Prepare for the next step. */
    acado_preparationStep();
  }
  /* Read the elapsed time. */
  real_t te = acado_toc(&t);

  if (VERBOSE)
    printf("\n\nEnd of the RTI loop. \n\n\n");

  /* Eye-candy. */

  if (!VERBOSE)
    printf("\n\n Average time of one real-time iteration:   %.3g microseconds\n\n", 1e6 * te / NUM_STEPS);

  acado_printDifferentialVariables();
  acado_printControlVariables();

  return 0;
}