Dear ACADO Commmunity,
Thank you for your great effort developing this good tool.
I’m trying to solve a MHE problem for parameter and disturbance estimation. The system is a vehicle , and has 6 differential states (3 position states and 3 velocities). I modelled the parameters and the disturbances (assuming constant disturbances) as differential state with zero time derivative, and is working fine. Is there another better way to do that?
Now, i need the disturbance to be free decision variables employing the acceleration measurements as Algebraic states to estimate disturbances with unknown model. I ignored the parameter estimation and focused only on disturbance estimation I formulated the problem as:
%%%%%%%%%%%%%%%%%%define the variables%%%%%%%%%%%%%%%%%%
DifferentialState x1 x2 x3 ; % Position and heading
DifferentialState x4 x5 x6 ; % Velocities
Control u1 u2 w1 w2 w3 ; % inputs of the system
Control w1 w2 w3 ; % disturbance
AlgebraicState au av ar ; % acceleration which are equal to dot(x4) dot(x5) dot(x6)
%%%%%%%%%%%%%%%%%%Intermediate States%%%%%%%%%%%%%%%%%%
xdu=is((m1)x5x6-(d1)x4);
xdv=is(-(m2)x4x6-(d2)x5);
xdr=is((m3)x4x5-(d3)x6);
xwu=is(m1uw1cos(x3)+m1uw2sin(x3));
xwv=is(-m2uw1sin(x3)+m2uw2cos(x3));
xwr=is((m3u)w3);
%%%%%%%%%%%%%%%%%%Define the DAE%%%%%%%%%%%%%%%%%%
Dear ACADO Commmunity,
Thank you for your great effort developing this good tool.
I’m trying to solve a MHE problem for parameter and disturbance estimation. The system is a vehicle , and has 6 differential states (3 position states and 3 velocities). I modelled the parameters and the disturbances (assuming constant disturbances) as differential state with zero time derivative, and is working fine. Is there another better way to do that?
Now, i need the disturbance to be free decision variables employing the acceleration measurements as Algebraic states to estimate disturbances with unknown model. I ignored the parameter estimation and focused only on disturbance estimation I formulated the problem as:
%%%%%%%%%%%%%%%%%%define the variables%%%%%%%%%%%%%%%%%%
DifferentialState x1 x2 x3 ; % Position and heading
DifferentialState x4 x5 x6 ; % Velocities
Control u1 u2 w1 w2 w3 ; % inputs of the system
Control w1 w2 w3 ; % disturbance
AlgebraicState au av ar ; % acceleration which are equal to dot(x4) dot(x5) dot(x6)
%%%%%%%%%%%%%%%%%%Intermediate States%%%%%%%%%%%%%%%%%%
xdu=is((m1)x5x6-(d1)x4);
xdv=is(-(m2)x4x6-(d2)x5);
xdr=is((m3)x4x5-(d3)x6);
xwu=is(m1uw1cos(x3)+m1uw2sin(x3));
xwv=is(-m2uw1sin(x3)+m2uw2cos(x3));
xwr=is((m3u)w3);
%%%%%%%%%%%%%%%%%%Define the DAE%%%%%%%%%%%%%%%%%%
f = acado.DifferentialEquation();
f.add(dot(x1) == x4cos(x3)-x5sin(x3));
f.add(dot(x2) == x4sin(x3)+x5cos(x3));
f.add(dot(x3) == x6);
f.add(dot(x4) == xdu+m1uu1+xwu);
f.add(dot(x5) == xdv+xwv);
f.add(dot(x6) == xdr +m3uu2+xwr);
f.add(0 == au-xdu-m1uu1-xwu);
f.add(0 == av-xdv-xwv);
f.add(0 == ar-xdr -m3uu2-xwr);
%%%%%%%%%%%%%%%%%%Measurment Function%%%%%%%%%%%%%%%%%%
h= [au;av;ar;x1;x2;x3;x4;x5; x6; u1; u2];
hN= [au;av;ar;x1;x2;x3;x4;x5; x6];
acadoSet('problemname', 'mhe');
ocp = acado.OCP( 0.0, N*Ts, N );
W_mat = eye(size(h,1));
WN_mat = eye(size(hN,1));
W = acado.BMatrix(W_mat);
WN = acado.BMatrix(WN_mat);
ocp.minimizeLSQ( W, h );
ocp.minimizeLSQEndTerm( WN, hN );
ocp.setModel(f);
mhe = acado.OCPexport( ocp );
mhe.set( 'HESSIAN_APPROXIMATION', 'GAUSS_NEWTON' );
mhe.set( 'DISCRETIZATION_TYPE', 'MULTIPLE_SHOOTING' );
mhe.set( 'INTEGRATOR_TYPE', 'INT_IRK_GL4' );
mhe.set( 'NUM_INTEGRATOR_STEPS', 1 *N );
mhe.set( 'QP_SOLVER', 'QP_QPOASES' );
mhe.set( 'HOTSTART_QP', 'YES' );
mhe.set( 'SPARSE_QP_SOLUTION', 'CONDENSING');
mhe.set( 'FIX_INITIAL_STATE ', 'NO'); %
mhe.set('QP_SOLVER', 'QP_QPOASES' );
mhe.set( 'CG_HARDCODE_CONSTRAINT_VALUES', 'YES' );
Unfortunately, i can not get convergent results even i simulate that with constant disturbance. Is there anything wrong with my formulation?
Thanks in advance
Hi Mohamed,
I am facing a similar problem now. Is there any update on this?
I tried to estimate the disturbance. To to do this, I defined it as the control input. However, the estimator did not converge.
Thanks.