From 58cfb759833b7a2d8bcda293c9e6ae6b642c11bc Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Mon, 28 Oct 2013 17:02:01 +0100 Subject: [PATCH 01/27] Added disturbance forces and torques and dummy control inputs If you want to use disturbance forces and torques, set the configuration parameter 'useDisturbanceForcesAndTorques' to true. They are added to the aero forces and torques. If you wish to do MHE with these disturbances, it is not possible, because the IMU_acceleration measurement function depends on these forces (and you cannot depend on both states and controls). Therefore, I added 3 dummy control inputs u_a1, u_a2 and u_a3. Enable them by setting 'useDummyAccelerationControls' to true. Now you can do a trick: In stead of doing: minize norm( h(x,u)-y ) which is not allowed, you can do in stead minimuze norm( u_a ) subject to h(x,u) - u_a == 0 which is allowed. --- rawe/models/carousel.py | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/rawe/models/carousel.py b/rawe/models/carousel.py index 23d4ee2..b5bbfe4 100644 --- a/rawe/models/carousel.py +++ b/rawe/models/carousel.py @@ -166,6 +166,20 @@ def getWind(): t2 = t2 * gamma_homotopy + dae.addZ('t2_homotopy') * (1 - gamma_homotopy) t3 = t3 * gamma_homotopy + dae.addZ('t3_homotopy') * (1 - gamma_homotopy) + # if we want disturbance forces and torques, add them as control inputs and add them to the forces and torques + if 'useDisturbanceForcesAndTorques' in conf and conf['useDisturbanceForcesAndTorques']: + f1 += dae.addU('f1_disturbance') + f2 += dae.addU('f2_disturbance') + f3 += dae.addU('f3_disturbance') + t1 += dae.addU('t1_disturbance') + t2 += dae.addU('t2_disturbance') + t3 += dae.addU('t3_disturbance') + + if 'useDummyAccelerationControls' in conf and conf['useDummyAccelerationControl']: + u_a1 = dae.addU('u_a1') + u_a2 = dae.addU('u_a2') + u_a3 = dae.addU('u_a3') + # mass matrix mm = C.SXMatrix(8, 8) mm[0,0] = jCarousel + m*rA*rA + m*x*x + m*y*y + 2*m*rA*x @@ -252,7 +266,6 @@ def getWind(): , t3 + w2*(j1*w1 + j31*w3) - j2*w1*w2 , ddr*r-(zt*w1*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)+zt*w2*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33))*(w3-ddelta*e33)-dx*(dx-zt*e21*(w1-ddelta*e13)+zt*e11*(w2-ddelta*e23))-dy*(dy-zt*e22*(w1-ddelta*e13)+zt*e12*(w2-ddelta*e23))-dz*(dz-zt*e23*(w1-ddelta*e13)+zt*e13*(w2-ddelta*e23))+dr*dr+(w1-ddelta*e13)*(e21*(zt*dx-zt2*e21*(w1-ddelta*e13)+zt2*e11*(w2-ddelta*e23))+e22*(zt*dy-zt2*e22*(w1-ddelta*e13)+zt2*e12*(w2-ddelta*e23))+zt*e23*(dz+zt*e13*w2-zt*e23*w1)+zt*e33*(w1*z+zt*e33*w1+ddelta*e11*x+ddelta*e12*y+zt*ddelta*e11*e31+zt*ddelta*e12*e32)+zt*e31*(x+zt*e31)*(w1-ddelta*e13)+zt*e32*(y+zt*e32)*(w1-ddelta*e13))-(w2-ddelta*e23)*(e11*(zt*dx-zt2*e21*(w1-ddelta*e13)+zt2*e11*(w2-ddelta*e23))+e12*(zt*dy-zt2*e22*(w1-ddelta*e13)+zt2*e12*(w2-ddelta*e23))+zt*e13*(dz+zt*e13*w2-zt*e23*w1)-zt*e33*(w2*z+zt*e33*w2+ddelta*e21*x+ddelta*e22*y+zt*ddelta*e21*e31+zt*ddelta*e22*e32)-zt*e31*(x+zt*e31)*(w2-ddelta*e23)-zt*e32*(y+zt*e32)*(w2-ddelta*e23)) ] ) - dRexp = C.SXMatrix(3,3) dRexp[0,0] = e21*(w3 - ddelta*e33) - e31*(w2 - ddelta*e23) @@ -303,7 +316,7 @@ def getWind(): dae['c'] = c dae['cdot'] = cdot dae['cddot'] = cddot - return (mm, rhs, dRexp) + return (mm, rhs, dRexp, dae) def carouselModel(conf): ''' @@ -420,7 +433,7 @@ def carouselModel(conf): -(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2) dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi - (massMatrix, rhs, dRexp) = setupModel(dae, conf) + (massMatrix, rhs, dRexp, dae) = setupModel(dae, conf) if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True: RotPole = 0.5 From 51bf7a53ab252edf029dd0ec2014cbfd7f943dc7 Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Mon, 28 Oct 2013 17:10:08 +0100 Subject: [PATCH 02/27] renamed dummy controls to dummyAcceleration_1, .. --- rawe/models/carousel.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rawe/models/carousel.py b/rawe/models/carousel.py index b5bbfe4..3b926e4 100644 --- a/rawe/models/carousel.py +++ b/rawe/models/carousel.py @@ -175,10 +175,10 @@ def getWind(): t2 += dae.addU('t2_disturbance') t3 += dae.addU('t3_disturbance') - if 'useDummyAccelerationControls' in conf and conf['useDummyAccelerationControl']: - u_a1 = dae.addU('u_a1') - u_a2 = dae.addU('u_a2') - u_a3 = dae.addU('u_a3') + if 'useDummyAccelerationControls' in conf and conf['useDummyAccelerationControls']: + u_a1 = dae.addU('dummyAcceleration_1') + u_a2 = dae.addU('dummyAcceleration_2') + u_a3 = dae.addU('dummyAcceleration_3') # mass matrix mm = C.SXMatrix(8, 8) From 60fcf5dc80f060b6aa9bd466565967d84aa49d05 Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Mon, 28 Oct 2013 17:31:22 +0100 Subject: [PATCH 03/27] Added disturbance forces and torques And also the dummy control inputs --- rawekite/carouselSteadyState.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/rawekite/carouselSteadyState.py b/rawekite/carouselSteadyState.py index b378877..c4d27b7 100644 --- a/rawekite/carouselSteadyState.py +++ b/rawekite/carouselSteadyState.py @@ -76,7 +76,10 @@ def constrainInvariantErrs(): 'daileron':0,'delevator':0,'drudder':0,'dflaps':0, 'nu':100,'motor_torque':0, 'dmotor_torque':0,'ddr':0, - 'dddr':0.0,'w0':0.0} + 'dddr':0.0,'w0':0.0, + 'f1_disturbance':0.0,'f2_disturbance':0.0,'f3_disturbance':0.0, + 't1_disturbance':0.0,'t2_disturbance':0.0,'t3_disturbance':0.0, + 'dummyAcceleration_1':0.0,'dummyAcceleration_2':0.0,'dummyAcceleration_3':0.0} dotGuess = {'x':0,'y':0,'z':0,'dx':0,'dy':0,'dz':0, 'r':0,'dr':0, 'e11':0,'e12':0,'e13':0, @@ -104,7 +107,10 @@ def constrainInvariantErrs(): 'daileron':(0,0),'delevator':(0,0),'drudder':(0,0),'dflaps':(0,0), 'nu':(0,3000),'motor_torque':(-1000,1000), 'ddr':(0,0), - 'dmotor_torque':(0,0),'dddr':(0,0),'w0':(0,0)} + 'dmotor_torque':(0,0),'dddr':(0,0),'w0':(0,0), + 'f1_disturbance':(0,0),'f2_disturbance':(0,0),'f3_disturbance':(0,0), + 't1_disturbance':(0,0),'t2_disturbance':(0,0),'t3_disturbance':(0,0), + 'dummyAcceleration_1':(0,0),'dummyAcceleration_2':(0,0),'dummyAcceleration_3':(0,0)} if ref_dict is not None: for name in ref_dict: bounds[name] = ref_dict[name] From a445da93259ba6334741c8a204738b6bb884cae6 Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Mon, 4 Nov 2013 16:49:51 +0100 Subject: [PATCH 04/27] added states and controls for kinematic IMU model --- rawe/models/carousel.py | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/rawe/models/carousel.py b/rawe/models/carousel.py index 3b926e4..e8b8342 100644 --- a/rawe/models/carousel.py +++ b/rawe/models/carousel.py @@ -180,6 +180,14 @@ def getWind(): u_a2 = dae.addU('dummyAcceleration_2') u_a3 = dae.addU('dummyAcceleration_3') + if 'kinematicIMUAccelerationModel' in conf and conf['kinematicIMUAccelerationModel']: + IMUAcceleration1 = dae.addU('IMUAcceleration1') + IMUAcceleration2 = dae.addU('IMUAcceleration2') + IMUAcceleration3 = dae.addU('IMUAcceleration3') + dx_IMU = dae.addX('dx_IMU') + dy_IMU = dae.addX('dy_IMU') + dz_IMU = dae.addX('dz_IMU') + # mass matrix mm = C.SXMatrix(8, 8) mm[0,0] = jCarousel + m*rA*rA + m*x*x + m*y*y + 2*m*rA*x @@ -461,6 +469,17 @@ def carouselModel(conf): ode = C.veccat([ode, dae.ddt('rudder') - dae['drudder']]) if 'flaps' in dae: ode = C.veccat([ode, dae.ddt('flaps') - dae['dflaps']]) + if 'kinematicIMUAccelerationModel' in conf and conf['kinematicIMUAccelerationModel']: + #dddelta = xDotSol['ddelta'] + # TODO effect of pIMU and dddelta + dddelta = 0.0 + R = dae['R_c2b'] + RIMU = conf['RIMU'] + IMUAcceleration = C.vertcat([dae['IMUAcceleration1'], dae['IMUAcceleration2'], dae['IMUAcceleration3']]) + ddp_IMU = C.mul(R.T,C.mul(RIMU.T,IMUAcceleration)) + dae['ddelta'] ** 2 * C.vertcat([dae['x'] + rA, dae['y'], 0]) - 2 * dae['ddelta'] * C.vertcat([-dae['dy'], dae'[dx'], 0]) - dddelta * C.vertcat([-dae['y'], dae['x'] + conf['rArm'], 0]) + C.vertcat([0, 0, conf['g']]) + ode = C.veccat([ode,dae.ddt('dx_IMU')-ddp_IMU[0], + dae.ddt('dy_IMU')-ddp_IMU[1], + dae.ddt('dz_IMU')-ddp_IMU[2]]) if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True: cPole = 0.5 From b5b7f4599d37a327a47e429ab6dc6d30f81a55d5 Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Wed, 6 Nov 2013 09:45:33 +0100 Subject: [PATCH 05/27] corrected typo --- rawe/models/carousel.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rawe/models/carousel.py b/rawe/models/carousel.py index e8b8342..6000c7b 100644 --- a/rawe/models/carousel.py +++ b/rawe/models/carousel.py @@ -476,7 +476,7 @@ def carouselModel(conf): R = dae['R_c2b'] RIMU = conf['RIMU'] IMUAcceleration = C.vertcat([dae['IMUAcceleration1'], dae['IMUAcceleration2'], dae['IMUAcceleration3']]) - ddp_IMU = C.mul(R.T,C.mul(RIMU.T,IMUAcceleration)) + dae['ddelta'] ** 2 * C.vertcat([dae['x'] + rA, dae['y'], 0]) - 2 * dae['ddelta'] * C.vertcat([-dae['dy'], dae'[dx'], 0]) - dddelta * C.vertcat([-dae['y'], dae['x'] + conf['rArm'], 0]) + C.vertcat([0, 0, conf['g']]) + ddp_IMU = C.mul(R.T,C.mul(RIMU.T,IMUAcceleration)) + dae['ddelta'] ** 2 * C.vertcat([dae['x'] + rA, dae['y'], 0]) - 2 * dae['ddelta'] * C.vertcat([-dae['dy'], dae['dx'], 0]) - dddelta * C.vertcat([-dae['y'], dae['x'] + conf['rArm'], 0]) + C.vertcat([0, 0, conf['g']]) ode = C.veccat([ode,dae.ddt('dx_IMU')-ddp_IMU[0], dae.ddt('dy_IMU')-ddp_IMU[1], dae.ddt('dz_IMU')-ddp_IMU[2]]) From 7eb372a2ae882b2dee7bb51d043f810e0cc09df2 Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Wed, 6 Nov 2013 18:01:41 +0100 Subject: [PATCH 06/27] Added code to do polynomial IMU measurements for kinematic model --- rawe/models/carousel.py | 37 ++++++++++++++++++++++++++++++++++++- 1 file changed, 36 insertions(+), 1 deletion(-) diff --git a/rawe/models/carousel.py b/rawe/models/carousel.py index 6000c7b..e8759af 100644 --- a/rawe/models/carousel.py +++ b/rawe/models/carousel.py @@ -476,7 +476,42 @@ def carouselModel(conf): R = dae['R_c2b'] RIMU = conf['RIMU'] IMUAcceleration = C.vertcat([dae['IMUAcceleration1'], dae['IMUAcceleration2'], dae['IMUAcceleration3']]) - ddp_IMU = C.mul(R.T,C.mul(RIMU.T,IMUAcceleration)) + dae['ddelta'] ** 2 * C.vertcat([dae['x'] + rA, dae['y'], 0]) - 2 * dae['ddelta'] * C.vertcat([-dae['dy'], dae['dx'], 0]) - dddelta * C.vertcat([-dae['y'], dae['x'] + conf['rArm'], 0]) + C.vertcat([0, 0, conf['g']]) + + if 'polynomialApproximationForIMUAcceleration' in conf and conf['polynomialApproximationForIMUAcceleration']: + # Create the Gram polynomials, the orthogonal polynomials on a discrete grid + # We need an extra state for time, and an extra control input such that we can have time go from -1 to 1 in each control interval + t = dae.addX('t') + ode = C.veccat([ode,dae.ddt('t')-conf('dt')]) + delta_t = dae.addU('delta_t') + t_interval = t + Nimu = conf['NumberOfIMUMeasurementsBetweenAbsoluteMeasurements'] + Npoly = conf['PolynomialOrder'] + m = Nimu-1 + gram = C.SXMatrix(Npoly+1,1) + gram[0,0] = 1.0/C.sqrt(m+1.0) + #Also do the linear function manually, cause it relies on gammanm = 1/infinity = 0, but which is not allowed + n=0.0 + alphanm = m/(n+1.0)*C.sqrt((4.0*(n+1.0)**2-1.0)/((m+1.0)**2-(n+1.0)**2)) + gram[1,0] = alphanm*t_interval*gram[0,0] + gammanm = 0.0 + for k in range(2,Npoly+1): + n = k-1.0 + alphanm = m/(n+1.0)*C.sqrt((4.0*(n+1.0)**2-1.0)/((m+1.0)**2-(n+1.0)**2)) + n = n-1.0 + alphanmm1 = m/(n+1.0)*C.sqrt((4.0*(n+1.0)**2-1.0)/((m+1.0)**2-(n+1.0)**2)) + gammanm = alphanm/alphanmm1 + gram[k,0] = alphanm*t_interval*gram[k-1,0]-gammanm*gram[k-2,0] + #Add the control inputs for the polynomial coefficients + polynomialCoefficients = C.SXMatrix(3,Npoly+1) + for k in range(Npoly+1): + polynomialCoefficients[0,k] = dae.addU('polynomialCoefficient_Acceleration_x_'+str(k)) + polynomialCoefficients[1,k] = dae.addU('polynomialCoefficient_Acceleration_y_'+str(k)) + polynomialCoefficients[2,k] = dae.addU('polynomialCoefficient_Acceleration_z_'+str(k)) + # Built the expression for the IMU Acceleration + IMUAcceleration = C.SXMatrix(3,1) + for k in range(Npoly+1): + IMUAcceleration += polynomialCoefficients[:,k]*gram[k,0] + ddp_IMU = C.mul(R.T,C.mul(RIMU.T,IMUAcceleration)) + dae['ddelta'] ** 2 * C.vertcat([dae['x'] + conf['rArm'], dae['y'], 0]) - 2 * dae['ddelta'] * C.vertcat([-dae['dy'], dae['dx'], 0]) - dddelta * C.vertcat([-dae['y'], dae['x'] + conf['rArm'], 0]) + C.vertcat([0, 0, conf['g']]) ode = C.veccat([ode,dae.ddt('dx_IMU')-ddp_IMU[0], dae.ddt('dy_IMU')-ddp_IMU[1], dae.ddt('dz_IMU')-ddp_IMU[2]]) From 96fa591cf8d2adc38e03c7b618bcd0fe09ea52a1 Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Thu, 7 Nov 2013 10:42:52 +0100 Subject: [PATCH 07/27] corrected typo --- rawe/models/carousel.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rawe/models/carousel.py b/rawe/models/carousel.py index e8759af..ca24652 100644 --- a/rawe/models/carousel.py +++ b/rawe/models/carousel.py @@ -481,7 +481,7 @@ def carouselModel(conf): # Create the Gram polynomials, the orthogonal polynomials on a discrete grid # We need an extra state for time, and an extra control input such that we can have time go from -1 to 1 in each control interval t = dae.addX('t') - ode = C.veccat([ode,dae.ddt('t')-conf('dt')]) + ode = C.veccat([ode,dae.ddt('t')-conf['dt']]) delta_t = dae.addU('delta_t') t_interval = t Nimu = conf['NumberOfIMUMeasurementsBetweenAbsoluteMeasurements'] From 8bc34b513092ff1bb8cd86e4fb6ac6ce1a54d38c Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Thu, 7 Nov 2013 13:44:37 +0100 Subject: [PATCH 08/27] Added test to check wether we use polys or not for IMUacc input --- rawe/models/carousel.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/rawe/models/carousel.py b/rawe/models/carousel.py index ca24652..a921124 100644 --- a/rawe/models/carousel.py +++ b/rawe/models/carousel.py @@ -181,9 +181,10 @@ def getWind(): u_a3 = dae.addU('dummyAcceleration_3') if 'kinematicIMUAccelerationModel' in conf and conf['kinematicIMUAccelerationModel']: - IMUAcceleration1 = dae.addU('IMUAcceleration1') - IMUAcceleration2 = dae.addU('IMUAcceleration2') - IMUAcceleration3 = dae.addU('IMUAcceleration3') + if not('polynomialApproximationForIMUAcceleration' in conf and conf['polynomialApproximationForIMUAcceleration']): + IMUAcceleration1 = dae.addU('IMUAcceleration1') + IMUAcceleration2 = dae.addU('IMUAcceleration2') + IMUAcceleration3 = dae.addU('IMUAcceleration3') dx_IMU = dae.addX('dx_IMU') dy_IMU = dae.addX('dy_IMU') dz_IMU = dae.addX('dz_IMU') @@ -475,8 +476,6 @@ def carouselModel(conf): dddelta = 0.0 R = dae['R_c2b'] RIMU = conf['RIMU'] - IMUAcceleration = C.vertcat([dae['IMUAcceleration1'], dae['IMUAcceleration2'], dae['IMUAcceleration3']]) - if 'polynomialApproximationForIMUAcceleration' in conf and conf['polynomialApproximationForIMUAcceleration']: # Create the Gram polynomials, the orthogonal polynomials on a discrete grid # We need an extra state for time, and an extra control input such that we can have time go from -1 to 1 in each control interval @@ -511,6 +510,8 @@ def carouselModel(conf): IMUAcceleration = C.SXMatrix(3,1) for k in range(Npoly+1): IMUAcceleration += polynomialCoefficients[:,k]*gram[k,0] + else: + IMUAcceleration = C.vertcat([dae['IMUAcceleration1'], dae['IMUAcceleration2'], dae['IMUAcceleration3']]) ddp_IMU = C.mul(R.T,C.mul(RIMU.T,IMUAcceleration)) + dae['ddelta'] ** 2 * C.vertcat([dae['x'] + conf['rArm'], dae['y'], 0]) - 2 * dae['ddelta'] * C.vertcat([-dae['dy'], dae['dx'], 0]) - dddelta * C.vertcat([-dae['y'], dae['x'] + conf['rArm'], 0]) + C.vertcat([0, 0, conf['g']]) ode = C.veccat([ode,dae.ddt('dx_IMU')-ddp_IMU[0], dae.ddt('dy_IMU')-ddp_IMU[1], From 471b327a10e537c7f46179326b39dffa192dfda7 Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Tue, 12 Nov 2013 13:57:28 +0100 Subject: [PATCH 09/27] Added bias for IMU acceleration measurements They are added as states, with derivative set to 0, such that they act as a parameter. --- rawe/models/carousel.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/rawe/models/carousel.py b/rawe/models/carousel.py index a921124..c97b5d0 100644 --- a/rawe/models/carousel.py +++ b/rawe/models/carousel.py @@ -189,6 +189,11 @@ def getWind(): dy_IMU = dae.addX('dy_IMU') dz_IMU = dae.addX('dz_IMU') + if 'useIMUAccelerationBias' in conf and conf['useIMUAccelerationBias']: + IMUAccelerationBias1 = dae.addX('IMUAccelerationBias1') + IMUAccelerationBias2 = dae.addX('IMUAccelerationBias2') + IMUAccelerationBias3 = dae.addX('IMUAccelerationBias3') + # mass matrix mm = C.SXMatrix(8, 8) mm[0,0] = jCarousel + m*rA*rA + m*x*x + m*y*y + 2*m*rA*x @@ -512,6 +517,12 @@ def carouselModel(conf): IMUAcceleration += polynomialCoefficients[:,k]*gram[k,0] else: IMUAcceleration = C.vertcat([dae['IMUAcceleration1'], dae['IMUAcceleration2'], dae['IMUAcceleration3']]) + if 'useIMUAccelerationBias' in conf and conf['useIMUAccelerationBias']: + IMUAccelerationBias = C.vertcat([dae['IMUAccelerationBias1'],dae['IMUAccelerationBias2'],dae['IMUAccelerationBias3']]) + IMUAcceleration += IMUAccelerationBias + ode = C.veccat([ode,dae.ddt('IMUAccelerationBias1')-0.0, + dae.ddt('IMUAccelerationBias2')-0.0, + dae.ddt('IMUAccelerationBias3')-0.0]) ddp_IMU = C.mul(R.T,C.mul(RIMU.T,IMUAcceleration)) + dae['ddelta'] ** 2 * C.vertcat([dae['x'] + conf['rArm'], dae['y'], 0]) - 2 * dae['ddelta'] * C.vertcat([-dae['dy'], dae['dx'], 0]) - dddelta * C.vertcat([-dae['y'], dae['x'] + conf['rArm'], 0]) + C.vertcat([0, 0, conf['g']]) ode = C.veccat([ode,dae.ddt('dx_IMU')-ddp_IMU[0], dae.ddt('dy_IMU')-ddp_IMU[1], From 3cbf1296318e0bd2d4fb9d9224f51542a9f61a61 Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Tue, 19 Nov 2013 09:49:26 +0100 Subject: [PATCH 10/27] Added gridpoints option to constraints --- rawe/ocp/Ocp.py | 5 ++++- rawe/ocp/writeAcadoOcpExport.py | 6 ++++++ 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/rawe/ocp/Ocp.py b/rawe/ocp/Ocp.py index f18fa21..62c6cf9 100644 --- a/rawe/ocp/Ocp.py +++ b/rawe/ocp/Ocp.py @@ -67,6 +67,7 @@ def __init__(self, dae, N=None, ts=None): self._constraints = [] self._constraintsStart = [] self._constraintsEnd = [] + self._constraintsWhen = [] self._dbgMessages = [] @@ -227,8 +228,10 @@ def maybeAddBoxConstraint(): self._constraintsEnd.append((lhs,comparison,rhs)) elif when is "AT_START": self._constraintsStart.append((lhs,comparison,rhs)) + elif isinstance(when,int) && 0 <= when <= self._N: + self._constraintsWhen.append((lhs,comparison,rhs,when)) else: - raise Exception("\"when\" must be 'AT_START' or 'AT_END', leaving it blank means always, you put: "+str(when)) + raise Exception("\"when\" must be 'AT_START', 'AT_END' or a gridpoint, leaving it blank means always, you put: "+str(when)) def minimizeLsq(self, obj): if isinstance(obj, list): diff --git a/rawe/ocp/writeAcadoOcpExport.py b/rawe/ocp/writeAcadoOcpExport.py index 6643887..07adc53 100644 --- a/rawe/ocp/writeAcadoOcpExport.py +++ b/rawe/ocp/writeAcadoOcpExport.py @@ -40,6 +40,10 @@ def writeAcadoAlgorithm(ocp, dae): k = len(outputs) outputs.append(rhs-lhs) constraintData.append((k,comparison,'AT_START')) + for (lhs,comparison,rhs,when) in ocp._constraintsWhen: + k = len(outputs) + outputs.append(rhs-lhs) + constraintData.append((k,comparison,when)) assert len(outputs) == len(constraintData), 'the "impossible" happened' # add dae residual and objectives @@ -271,6 +275,8 @@ def generateAcadoOcp(ocp, integratorOptions, ocpOptions): whenStr = 'AT_END, ' elif when == 'AT_START': whenStr = 'AT_START, ' + elif isinstance(when,int): + whenStr = str(when)+', ' else: raise Exception('the "impossible" happened, unrecognized "when": '+str(when)) lines.append( From 48c43d49130f7e91d0bf9408139a45fa08f23720 Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Tue, 19 Nov 2013 09:50:52 +0100 Subject: [PATCH 11/27] Removed typo: && -> & --- rawe/ocp/Ocp.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rawe/ocp/Ocp.py b/rawe/ocp/Ocp.py index 62c6cf9..4e0c5cd 100644 --- a/rawe/ocp/Ocp.py +++ b/rawe/ocp/Ocp.py @@ -228,7 +228,7 @@ def maybeAddBoxConstraint(): self._constraintsEnd.append((lhs,comparison,rhs)) elif when is "AT_START": self._constraintsStart.append((lhs,comparison,rhs)) - elif isinstance(when,int) && 0 <= when <= self._N: + elif isinstance(when,int) & 0 <= when <= self._N: self._constraintsWhen.append((lhs,comparison,rhs,when)) else: raise Exception("\"when\" must be 'AT_START', 'AT_END' or a gridpoint, leaving it blank means always, you put: "+str(when)) From 86e96cb5fb85786c18e8a5cbd17a9e42f5b9987f Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Thu, 21 Nov 2013 17:11:27 +0100 Subject: [PATCH 12/27] Fixed bug: imu bias should be substracted from measurements --- rawe/models/carousel.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rawe/models/carousel.py b/rawe/models/carousel.py index c97b5d0..21818c9 100644 --- a/rawe/models/carousel.py +++ b/rawe/models/carousel.py @@ -519,7 +519,7 @@ def carouselModel(conf): IMUAcceleration = C.vertcat([dae['IMUAcceleration1'], dae['IMUAcceleration2'], dae['IMUAcceleration3']]) if 'useIMUAccelerationBias' in conf and conf['useIMUAccelerationBias']: IMUAccelerationBias = C.vertcat([dae['IMUAccelerationBias1'],dae['IMUAccelerationBias2'],dae['IMUAccelerationBias3']]) - IMUAcceleration += IMUAccelerationBias + IMUAcceleration -= IMUAccelerationBias ode = C.veccat([ode,dae.ddt('IMUAccelerationBias1')-0.0, dae.ddt('IMUAccelerationBias2')-0.0, dae.ddt('IMUAccelerationBias3')-0.0]) From 9ecd0cff95e413b6afd8bc574c3c662ae8dcb65a Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Fri, 22 Nov 2013 11:55:06 +0100 Subject: [PATCH 13/27] removed bug: t for poly shoudl take delta_t in account --- rawe/models/carousel.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rawe/models/carousel.py b/rawe/models/carousel.py index 21818c9..0a9795d 100644 --- a/rawe/models/carousel.py +++ b/rawe/models/carousel.py @@ -487,7 +487,7 @@ def carouselModel(conf): t = dae.addX('t') ode = C.veccat([ode,dae.ddt('t')-conf['dt']]) delta_t = dae.addU('delta_t') - t_interval = t + t_interval = t-delta_t Nimu = conf['NumberOfIMUMeasurementsBetweenAbsoluteMeasurements'] Npoly = conf['PolynomialOrder'] m = Nimu-1 From 638dfda89d9cb9326ae1e49ec9e99fa717c9dfd3 Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Thu, 28 Nov 2013 16:00:31 +0100 Subject: [PATCH 14/27] Added disturbance turbulence if in conf --- rawe/models/carousel.py | 27 +++++++++++++++++++++++---- 1 file changed, 23 insertions(+), 4 deletions(-) diff --git a/rawe/models/carousel.py b/rawe/models/carousel.py index 0a9795d..2f76d8a 100644 --- a/rawe/models/carousel.py +++ b/rawe/models/carousel.py @@ -145,14 +145,33 @@ def getWind(): , dy + ddelta*(rA + x) , dz ]) - C.veccat([dae['cos_delta']*wind_x, dae['sin_delta']*wind_x, 0]) + if 'useDisturbanceTurbulence' in conf and conf['useDisturbanceTurbulence']: + Psi_1 = dae.addU('Psi_1') + Psi_2 = dae.addU('Psi_2') + Psi_3 = dae.addU('Psi_3') + Omega_1 = dae.addU('Omega_1') + Omega_2 = dae.addU('Omega_2') + Omega_3 = dae.addU('Omega_3') + v_bw_c += C.veccat([Psi_1, + Psi_2, + Psi_3]) + dae['w_bn_b_with_disturbance'] = dae['w_bn_b'] - C.veccat([Omega_1, + Omega_2, + Omega_3]) # Velocity of aircraft w.r.t wind carrying frame, expressed in body frame # (needed to compute the aero forces and torques !) v_bw_b = C.mul( dae['R_c2b'], v_bw_c ) - (f1, f2, f3, t1, t2, t3) = aeroForcesTorques(dae, conf, v_bw_c, v_bw_b, - dae['w_bn_b'], - (dae['e21'], dae['e22'], dae['e23']) # y-axis of body frame in carousel coordinates - ) + if 'useDisturbanceTurbulence' in conf and conf['useDisturbanceTurbulence']: + (f1, f2, f3, t1, t2, t3) = aeroForcesTorques(dae, conf, v_bw_c, v_bw_b, + dae['w_bn_b_with_disturbance'], + (dae['e21'], dae['e22'], dae['e23']) # y-axis of body frame in carousel coordinates + ) + else: + (f1, f2, f3, t1, t2, t3) = aeroForcesTorques(dae, conf, v_bw_c, v_bw_b, + dae['w_bn_b'], + (dae['e21'], dae['e22'], dae['e23']) # y-axis of body frame in carousel coordinates + ) # f1..f3 expressed in carrousel coordinates # t1..t3 expressed in body coordinates From b1dc0e6806663a604dbba36a026dcbf59a74454d Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Tue, 3 Dec 2013 14:05:04 +0100 Subject: [PATCH 15/27] Added option for computing covariance --- rawe/ocp/Ocp.py | 1 + 1 file changed, 1 insertion(+) diff --git a/rawe/ocp/Ocp.py b/rawe/ocp/Ocp.py index 4e0c5cd..cea3671 100644 --- a/rawe/ocp/Ocp.py +++ b/rawe/ocp/Ocp.py @@ -38,6 +38,7 @@ def __init__(self): self.add(OptDouble('LEVENBERG_MARQUARDT',default=0.0)) self.add(OptBool('CG_USE_ARRIVAL_COST',default=False)) self.add(OptBool('CG_USE_VARIABLE_WEIGHTING_MATRIX',default=False)) + self.add(OptBool('CG_COMPUTE_COVARIANCE_MATRIX',default=False)) # self.add(OptBool('CG_USE_C99',default=True)) class Ocp(object): From 79a5f5a32b8cc18c3ed46f459abba13fa54a165a Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Wed, 4 Dec 2013 09:19:59 +0100 Subject: [PATCH 16/27] Added functionality to access the computed covariance matrix, named sigmaN --- rawe/ocp/ocg_interface.py | 7 +++++++ rawe/ocp/ocprt.py | 7 ++++++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/rawe/ocp/ocg_interface.py b/rawe/ocp/ocg_interface.py index 38eadff..433f95d 100644 --- a/rawe/ocp/ocg_interface.py +++ b/rawe/ocp/ocg_interface.py @@ -117,6 +117,13 @@ return memcpyMat(val, acadoVariables.SN, nr, nc, ACADO_NYN, ACADO_NYN); } #endif /* ACADO_WEIGHTING_MATRICES_TYPE */ +#if ACADO_COMPUTE_COVARIANCE_MATRIX +int py_set_sigmaN(real_t * val, const int nr, const int nc){ + return memcpyMat(acadoVariables.sigmaN, val, nr, nc, ACADO_NX, ACADO_NX); } +int py_get_sigmaN(real_t * val, const int nr, const int nc){ + return memcpyMat(val, acadoVariables.sigmaN, nr, nc, ACADO_NX, ACADO_NX); } +#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */ + #if ACADO_USE_ARRIVAL_COST == 1 int py_set_SAC(real_t * val, const int nr, const int nc){ return memcpyMat(acadoVariables.SAC, val, nr, nc, ACADO_NX, ACADO_NX); } diff --git a/rawe/ocp/ocprt.py b/rawe/ocp/ocprt.py index 3e3fe74..9b918f1 100644 --- a/rawe/ocp/ocprt.py +++ b/rawe/ocp/ocprt.py @@ -52,7 +52,7 @@ def blah(self,*args,**kwargs): return blah class OcpRT(object): - _canonicalNames = ['x','u','z','y','yN','x0','S','SN','SAC','xAC','WL'] + _canonicalNames = ['x','u','z','y','yN','x0','S','SN','SAC','xAC','WL','sigmaN'] @property def ocp(self): @@ -98,6 +98,9 @@ def __init__(self, ocp, self._lib.py_get_ACADO_NU())) self.y = numpy.zeros((self._lib.py_get_ACADO_N(), self._lib.py_get_ACADO_NY())) + if ocpOptions["CG_COMPUTE_COVARIANCE_MATRIX"]: + self.sigmaN = numpy.zeros((self._lib.py_get_ACADO_NX(), + self._lib.py_get_ACADO_NX())) if self._lib.py_get_ACADO_NXA() > 0: self.z = numpy.zeros((self._lib.py_get_ACADO_N(), self._lib.py_get_ACADO_NXA())) @@ -201,6 +204,7 @@ def _setAll(self): self._callMat(self._lib.py_set_yN, self.yN) self._callMat(self._lib.py_set_S, self.S) self._callMat(self._lib.py_set_SN, self.SN) + self._callMat(self._lib.py_set_sigmaN, self.sigmaN) if self._lib.py_get_ACADO_INITIAL_STATE_FIXED(): self._callMat(self._lib.py_set_x0, self.x0) if self._lib.py_get_ACADO_USE_ARRIVAL_COST(): @@ -217,6 +221,7 @@ def _getAll(self): self._callMat(self._lib.py_get_yN, self.yN) self._callMat(self._lib.py_get_S, self.S) self._callMat(self._lib.py_get_SN, self.SN) + self._callMat(self._lib.py_get_sigmaN, self.sigmaN) if self._lib.py_get_ACADO_INITIAL_STATE_FIXED(): self._callMat(self._lib.py_get_x0, self.x0) if self._lib.py_get_ACADO_USE_ARRIVAL_COST(): From acac422639aa783668f4f011be1381fffeef832e Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Thu, 5 Dec 2013 16:22:48 +0100 Subject: [PATCH 17/27] Corrected use of SAC, such that is is only added when it is needed --- rawe/ocp/ocg_interface.py | 2 ++ rawe/ocp/ocprt.py | 6 ++++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/rawe/ocp/ocg_interface.py b/rawe/ocp/ocg_interface.py index 433f95d..9f5c26c 100644 --- a/rawe/ocp/ocg_interface.py +++ b/rawe/ocp/ocg_interface.py @@ -164,6 +164,8 @@ int py_get_ACADO_QPDUNES(void){ return ACADO_QPDUNES; } /** Indicator for determining the QP solver used by the ACADO solver code. */ int py_get_ACADO_QP_SOLVER(void){ return ACADO_QP_SOLVER; } +/** Indicator for computing covariance matrix. */ +int py_get_ACADO_COMPUTE_COVARIANCE_MATRIX(void){ return ACADO_COMPUTE_COVARIANCE_MATRIX; } /** Indicator for fixed initial state. */ int py_get_ACADO_INITIAL_STATE_FIXED(void){ return ACADO_INITIAL_STATE_FIXED; } /** Indicator for type of fixed weighting matrices. */ diff --git a/rawe/ocp/ocprt.py b/rawe/ocp/ocprt.py index 9b918f1..3fa364d 100644 --- a/rawe/ocp/ocprt.py +++ b/rawe/ocp/ocprt.py @@ -204,7 +204,8 @@ def _setAll(self): self._callMat(self._lib.py_set_yN, self.yN) self._callMat(self._lib.py_set_S, self.S) self._callMat(self._lib.py_set_SN, self.SN) - self._callMat(self._lib.py_set_sigmaN, self.sigmaN) + if self._lib.py_get_ACADO_COMPUTE_COVARIANCE_MATRIX(): + self._callMat(self._lib.py_set_sigmaN, self.sigmaN) if self._lib.py_get_ACADO_INITIAL_STATE_FIXED(): self._callMat(self._lib.py_set_x0, self.x0) if self._lib.py_get_ACADO_USE_ARRIVAL_COST(): @@ -221,7 +222,8 @@ def _getAll(self): self._callMat(self._lib.py_get_yN, self.yN) self._callMat(self._lib.py_get_S, self.S) self._callMat(self._lib.py_get_SN, self.SN) - self._callMat(self._lib.py_get_sigmaN, self.sigmaN) + if self._lib.py_get_ACADO_COMPUTE_COVARIANCE_MATRIX(): + self._callMat(self._lib.py_get_sigmaN, self.sigmaN) if self._lib.py_get_ACADO_INITIAL_STATE_FIXED(): self._callMat(self._lib.py_get_x0, self.x0) if self._lib.py_get_ACADO_USE_ARRIVAL_COST(): From bb8e73ce516bffe505b48b6c6fba8a95fa56be75 Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Wed, 19 Feb 2014 15:00:58 +0100 Subject: [PATCH 18/27] Corrected turbulence: now in inertial frame rather than in rotating --- rawe/models/carousel.py | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/rawe/models/carousel.py b/rawe/models/carousel.py index 2f76d8a..46b96c1 100644 --- a/rawe/models/carousel.py +++ b/rawe/models/carousel.py @@ -152,12 +152,17 @@ def getWind(): Omega_1 = dae.addU('Omega_1') Omega_2 = dae.addU('Omega_2') Omega_3 = dae.addU('Omega_3') - v_bw_c += C.veccat([Psi_1, - Psi_2, - Psi_3]) - dae['w_bn_b_with_disturbance'] = dae['w_bn_b'] - C.veccat([Omega_1, + # Reference to carousel frame + R_r2c = C.blockcat([[dae['cos_delta'],dae['sin_delta'],0.0], + [-dae['sin_delta'],dae['cos_delta'],0.0], + [0.0,0.0,1.0]]) + + v_bw_c += C.mul(R_r2c, C.veccat([Psi_1, + Psi_2, + Psi_3])) + dae['w_bn_b_with_disturbance'] = dae['w_bn_b'] - C.mul([dae['R_c2b'],R_r2c,C.veccat([Omega_1, Omega_2, - Omega_3]) + Omega_3])]) # Velocity of aircraft w.r.t wind carrying frame, expressed in body frame # (needed to compute the aero forces and torques !) v_bw_b = C.mul( dae['R_c2b'], v_bw_c ) From f322797438d93d1365680bc10827c2bdc1157af1 Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Wed, 19 Feb 2014 15:01:42 +0100 Subject: [PATCH 19/27] Updated mass of airplane after installation of new servos --- rawe/models/arianne_conf.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rawe/models/arianne_conf.py b/rawe/models/arianne_conf.py index 9c1e862..b0282ba 100644 --- a/rawe/models/arianne_conf.py +++ b/rawe/models/arianne_conf.py @@ -71,7 +71,7 @@ def makeConf(): #[kite] - 'mass': 0.626, # mass of the kite # [ kg] + 'mass': 0.685, # mass of the kite # [ kg] 'tether_mass': 0.0, # use aerodynamic approximations instead of triginometry From 0424123cc97917ee7b3226453b77d41770b755de Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Fri, 28 Feb 2014 17:51:42 +0100 Subject: [PATCH 20/27] Added scaled disturbance forces and torques --- rawe/models/aero.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/rawe/models/aero.py b/rawe/models/aero.py index d62c366..fd6352f 100644 --- a/rawe/models/aero.py +++ b/rawe/models/aero.py @@ -167,6 +167,14 @@ def aeroForcesTorques(dae, conf, v_bw_f, v_bw_b, w_bn_b, (eTe_f_1, eTe_f_2, eTe_ t_b_2 = rho_sref_v2*cref*dae['cm_small'] t_b_3 = rho_sref_v2*bref*dae['cn_small'] + if 'useScaledDisturbanceForcesAndTorques' in conf and conf['useScaledDisturbanceForcesAndTorques']: + f_f[0] += rho_sref_v2*dae['f1_disturbance_scaled'] + f_f[1] += rho_sref_v2*dae['f2_disturbance_scaled'] + f_f[2] += rho_sref_v2*dae['f3_disturbance_scaled'] + t_b_1 += rho_sref_v2*dae['t1_disturbance_scaled'] + t_b_2 += rho_sref_v2*dae['t2_disturbance_scaled'] + t_b_3 += rho_sref_v2*dae['t3_disturbance_scaled'] + dae['aero_fx'] = f_f[0] dae['aero_fy'] = f_f[1] dae['aero_fz'] = f_f[2] From abb17e02e7fd68e8a7701b465e37be4e32a016e9 Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Fri, 28 Feb 2014 17:52:33 +0100 Subject: [PATCH 21/27] Added comments, scaled dist. forces --- rawe/models/carousel.py | 29 +++++++++++++++++++++-------- 1 file changed, 21 insertions(+), 8 deletions(-) diff --git a/rawe/models/carousel.py b/rawe/models/carousel.py index 46b96c1..254e3fb 100644 --- a/rawe/models/carousel.py +++ b/rawe/models/carousel.py @@ -152,17 +152,30 @@ def getWind(): Omega_1 = dae.addU('Omega_1') Omega_2 = dae.addU('Omega_2') Omega_3 = dae.addU('Omega_3') - # Reference to carousel frame - R_r2c = C.blockcat([[dae['cos_delta'],dae['sin_delta'],0.0], - [-dae['sin_delta'],dae['cos_delta'],0.0], - [0.0,0.0,1.0]]) - - v_bw_c += C.mul(R_r2c, C.veccat([Psi_1, + ## Reference to carousel frame + #R_r2c = C.blockcat([[dae['cos_delta'],dae['sin_delta'],0.0], + #[-dae['sin_delta'],dae['cos_delta'],0.0], + #[0.0,0.0,1.0]]) + # Add linear turbulence to velocity. They are assumed to be in the inertial frame + #v_bw_c -= C.mul(R_r2c, C.veccat([Psi_1, + #Psi_2, + #Psi_3])) + v_bw_c -= C.veccat([Psi_1, Psi_2, - Psi_3])) + Psi_3]) + ## Add rotational turbulence to w_bn_b. Note that the turbulence is assumed to be in the inertial frame dae['w_bn_b_with_disturbance'] = dae['w_bn_b'] - C.mul([dae['R_c2b'],R_r2c,C.veccat([Omega_1, Omega_2, Omega_3])]) + + if 'useScaledDisturbanceForcesAndTorques' in conf and conf['useScaledDisturbanceForcesAndTorques']: + f1_scaled = dae.addU('f1_disturbance_scaled') + f2_scaled = dae.addU('f2_disturbance_scaled') + f3_scaled = dae.addU('f3_disturbance_scaled') + t1_scaled = dae.addU('t1_disturbance_scaled') + t2_scaled = dae.addU('t2_disturbance_scaled') + t3_scaled = dae.addU('t3_disturbance_scaled') + # Velocity of aircraft w.r.t wind carrying frame, expressed in body frame # (needed to compute the aero forces and torques !) v_bw_b = C.mul( dae['R_c2b'], v_bw_c ) @@ -190,7 +203,7 @@ def getWind(): t2 = t2 * gamma_homotopy + dae.addZ('t2_homotopy') * (1 - gamma_homotopy) t3 = t3 * gamma_homotopy + dae.addZ('t3_homotopy') * (1 - gamma_homotopy) - # if we want disturbance forces and torques, add them as control inputs and add them to the forces and torques + # if we want disturbance forces and torques, add them as control inputs and add them to the forces and torques if 'useDisturbanceForcesAndTorques' in conf and conf['useDisturbanceForcesAndTorques']: f1 += dae.addU('f1_disturbance') f2 += dae.addU('f2_disturbance') From 35456d4eaede067a763cfa1f01b278f83c8b8138 Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Tue, 8 Apr 2014 10:46:35 +0200 Subject: [PATCH 22/27] Small change to cd0 to correct offset in dist. F. --- rawe/models/arianne_conf.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/rawe/models/arianne_conf.py b/rawe/models/arianne_conf.py index b0282ba..c777b5c 100644 --- a/rawe/models/arianne_conf.py +++ b/rawe/models/arianne_conf.py @@ -32,7 +32,9 @@ def makeConf(): 'cD_A': 0.027310, 'cD_A2': 1.948549, 'cD_B2': -0.013039, - 'cD0': 0.006600, + #'cD0': 0.006600, + #'cD0': 0.495, + 'cD0': 0.107, 'cY_B': -0.131871, @@ -85,8 +87,8 @@ def makeConf(): 'bref': 0.96, #sqrt(sref*AR) 'cref': 0.128, #sqrt(sref/AR) - 'zt': 0.01, - 'xt': 0.018, # quick fix + 'zt': 0.025, + 'xt': 0.0, # quick fix #INERTIA MATRIX (Kurt's direct measurements) 'j1': 0.0163, From 1d712ab88981cf8d0ec2d70958b9096f2a04962c Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Thu, 22 May 2014 16:41:30 +0200 Subject: [PATCH 23/27] Added order 0 polynomial --- rawe/models/carousel.py | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/rawe/models/carousel.py b/rawe/models/carousel.py index 254e3fb..1704cbc 100644 --- a/rawe/models/carousel.py +++ b/rawe/models/carousel.py @@ -530,18 +530,19 @@ def carouselModel(conf): m = Nimu-1 gram = C.SXMatrix(Npoly+1,1) gram[0,0] = 1.0/C.sqrt(m+1.0) - #Also do the linear function manually, cause it relies on gammanm = 1/infinity = 0, but which is not allowed - n=0.0 - alphanm = m/(n+1.0)*C.sqrt((4.0*(n+1.0)**2-1.0)/((m+1.0)**2-(n+1.0)**2)) - gram[1,0] = alphanm*t_interval*gram[0,0] - gammanm = 0.0 - for k in range(2,Npoly+1): - n = k-1.0 + if Npoly>0: + #Also do the linear function manually, cause it relies on gammanm = 1/infinity = 0, but which is not allowed + n=0.0 alphanm = m/(n+1.0)*C.sqrt((4.0*(n+1.0)**2-1.0)/((m+1.0)**2-(n+1.0)**2)) - n = n-1.0 - alphanmm1 = m/(n+1.0)*C.sqrt((4.0*(n+1.0)**2-1.0)/((m+1.0)**2-(n+1.0)**2)) - gammanm = alphanm/alphanmm1 - gram[k,0] = alphanm*t_interval*gram[k-1,0]-gammanm*gram[k-2,0] + gram[1,0] = alphanm*t_interval*gram[0,0] + gammanm = 0.0 + for k in range(2,Npoly+1): + n = k-1.0 + alphanm = m/(n+1.0)*C.sqrt((4.0*(n+1.0)**2-1.0)/((m+1.0)**2-(n+1.0)**2)) + n = n-1.0 + alphanmm1 = m/(n+1.0)*C.sqrt((4.0*(n+1.0)**2-1.0)/((m+1.0)**2-(n+1.0)**2)) + gammanm = alphanm/alphanmm1 + gram[k,0] = alphanm*t_interval*gram[k-1,0]-gammanm*gram[k-2,0] #Add the control inputs for the polynomial coefficients polynomialCoefficients = C.SXMatrix(3,Npoly+1) for k in range(Npoly+1): From c70e7483272cb564de5a391b9bd66ce9eafcb2e2 Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Mon, 2 Jun 2014 17:17:49 +0200 Subject: [PATCH 24/27] Added polynomials for omega to model --- rawe/models/carousel.py | 122 +++++++++++++++++++++++++++++++++++++++- 1 file changed, 121 insertions(+), 1 deletion(-) diff --git a/rawe/models/carousel.py b/rawe/models/carousel.py index 1704cbc..dd51ec6 100644 --- a/rawe/models/carousel.py +++ b/rawe/models/carousel.py @@ -512,7 +512,126 @@ def carouselModel(conf): ode = C.veccat([ode, dae.ddt('rudder') - dae['drudder']]) if 'flaps' in dae: ode = C.veccat([ode, dae.ddt('flaps') - dae['dflaps']]) - if 'kinematicIMUAccelerationModel' in conf and conf['kinematicIMUAccelerationModel']: + + if 'polynomialApproximationForIMUAngularVelocity' in conf and conf['polynomialApproximationForIMUAngularVelocity']:#{{{ + #Add the control inputs for the polynomial coefficients. That's all we have to do here. The rest is taken care of by the constraints in the MHE formulation + Npoly = conf['PolynomialOrder'] + polynomialCoefficients_AngularVelocity = C.SXMatrix(3,Npoly+1) + for k in range(Npoly+1): + polynomialCoefficients_AngularVelocity[0,k] = dae.addU('polynomialCoefficient_AngularVelocity_x_'+str(k)) + polynomialCoefficients_AngularVelocity[1,k] = dae.addU('polynomialCoefficient_AngularVelocity_y_'+str(k)) + polynomialCoefficients_AngularVelocity[2,k] = dae.addU('polynomialCoefficient_AngularVelocity_z_'+str(k)) + + dae.addX('IMUAngularVelocity_dummy_x') + dae.addX('IMUAngularVelocity_dummy_y') + dae.addX('IMUAngularVelocity_dummy_z') + + # Define th polynomial #{{{ + Nimu = conf['NumberOfIMUMeasurementsBetweenAbsoluteMeasurements'] + m = Nimu-1 + tt = C.ssym('tt') + gram = C.SXMatrix(Npoly+1,1) + gram[0,0] = 1.0/C.sqrt(m+1.0) + if Npoly>0: + #Also do the linear function manually, cause it relies on gammanm = 1/infinity = 0, but which is not allowed + n=0.0 + alphanm = m/(n+1.0)*C.sqrt((4.0*(n+1.0)**2-1.0)/((m+1.0)**2-(n+1.0)**2)) + gram[1,0] = alphanm*tt*gram[0,0] + gammanm = 0.0 + for k in range(2,Npoly+1): + n = k-1.0 + alphanm = m/(n+1.0)*C.sqrt((4.0*(n+1.0)**2-1.0)/((m+1.0)**2-(n+1.0)**2)) + n = n-1.0 + alphanmm1 = m/(n+1.0)*C.sqrt((4.0*(n+1.0)**2-1.0)/((m+1.0)**2-(n+1.0)**2)) + gammanm = alphanm/alphanmm1 + gram[k,0] = alphanm*tt*gram[k-1,0]-gammanm*gram[k-2,0] + Gram = C.SXFunction([tt],[gram]) + Gram.init() + Gram.setInput(-1.0) + Gram.evaluate() + polyEvalStart = Gram.getOutput() + Gram.setInput(-1.0+2.0*Nimu/m) + Gram.evaluate() + polyEvalEnd = Gram.getOutput()#}}} + + #{{{ + w_bn_imu_start_x = 0.0 + w_bn_imu_start_y = 0.0 + w_bn_imu_start_z = 0.0 + + w_bn_imu_end_x = 0.0 + w_bn_imu_end_y = 0.0 + w_bn_imu_end_z = 0.0 + for k in range(Npoly+1): + w_bn_imu_start_x += dae['polynomialCoefficient_AngularVelocity_x_'+str(k)]*polyEvalStart[k] + w_bn_imu_start_y += dae['polynomialCoefficient_AngularVelocity_y_'+str(k)]*polyEvalStart[k] + w_bn_imu_start_z += dae['polynomialCoefficient_AngularVelocity_z_'+str(k)]*polyEvalStart[k] + + w_bn_imu_end_x += dae['polynomialCoefficient_AngularVelocity_x_'+str(k)]*polyEvalEnd[k] + w_bn_imu_end_y += dae['polynomialCoefficient_AngularVelocity_y_'+str(k)]*polyEvalEnd[k] + w_bn_imu_end_z += dae['polynomialCoefficient_AngularVelocity_z_'+str(k)]*polyEvalEnd[k]#}}} + + ode = C.veccat([ode,dae.ddt('IMUAngularVelocity_dummy_x')-(w_bn_imu_end_x-w_bn_imu_start_x)/conf['Ts'], + dae.ddt('IMUAngularVelocity_dummy_y')-(w_bn_imu_end_y-w_bn_imu_start_y)/conf['Ts'], + dae.ddt('IMUAngularVelocity_dummy_z')-(w_bn_imu_end_z-w_bn_imu_start_z)/conf['Ts']]) + IMUAngularVelocity_dummy = C.vertcat([dae['IMUAngularVelocity_dummy_x'],dae['IMUAngularVelocity_dummy_y'],dae['IMUAngularVelocity_dummy_z']]) + + BodyAngularVelocity_dummy = C.mul(conf['RIMU'].T,IMUAngularVelocity_dummy) + dae['BodyAngularVelocity_dummy_x'] = BodyAngularVelocity_dummy[0,0] + dae['BodyAngularVelocity_dummy_y'] = BodyAngularVelocity_dummy[1,0] + dae['BodyAngularVelocity_dummy_z'] = BodyAngularVelocity_dummy[2,0] + + dae['w_bn_imu_start_x'] = w_bn_imu_start_x + dae['w_bn_imu_start_y'] = w_bn_imu_start_y + dae['w_bn_imu_start_z'] = w_bn_imu_start_z + + dae['w_bn_imu_end_x'] = w_bn_imu_end_x + dae['w_bn_imu_end_y'] = w_bn_imu_end_y + dae['w_bn_imu_end_z'] = w_bn_imu_end_z + + dae['Output1'] = dae['w_bn_b_x'] - dae['BodyAngularVelocity_dummy_x'] + dae['Output2'] = dae['w_bn_b_y'] - dae['BodyAngularVelocity_dummy_y'] + dae['Output3'] = dae['w_bn_b_z'] - dae['BodyAngularVelocity_dummy_z'] + + #if 0:#{{{ + #assert conf['polynomialApproximationForIMUAcceleration'], 'At the moment, only polynomials for angular velocity are not supported. Please do proper adding of t and delta_t, and create the polynomials (gram) here if you want to do it.' + ## Built the expression for the IMU Angular Velocity + #IMUAngularVelocity = C.SXMatrix(3,1) + #for k in range(Npoly+1): + #IMUAngularVelocity += polynomialCoefficients_AngularVelocity[:,k]*gram[k,0] + #w_bn_b_IMU = C.mul(conf['RIMU'].T, IMUAngularVelocity) + #[e11_IMU,e12_IMU,e13_IMU,e21_IMU,e22_IMU,e23_IMU,e31_IMU,e32_IMU,e33_IMU] = dae.addX(['e11_IMU','e12_IMU','e13_IMU','e21_IMU','e22_IMU','e23_IMU','e31_IMU','e32_IMU','e33_IMU']) + #dae['R_c2b_IMU'] = C.blockcat([[e11_IMU,e12_IMU,e13_IMU], + #[e21_IMU,e22_IMU,e23_IMU], + #[e31_IMU,e32_IMU,e33_IMU]]) + #dRexp_IMU = C.SXMatrix(3,3) + #dRexp_IMU[0,0] = e21_IMU*(w_bn_b_IMU[2] - dae['ddelta']*e33_IMU) - e31_IMU*(w_bn_b_IMU[1] - dae['ddelta']*e23_IMU) + #dRexp_IMU[0,1] = e31_IMU*(w_bn_b_IMU[0] - dae['ddelta']*e13_IMU) - e11_IMU*(w_bn_b_IMU[2] - dae['ddelta']*e33_IMU) + #dRexp_IMU[0,2] = e11_IMU*(w_bn_b_IMU[1] - dae['ddelta']*e23_IMU) - e21_IMU*(w_bn_b_IMU[0] - dae['ddelta']*e13_IMU) + #dRexp_IMU[1,0] = e22_IMU*(w_bn_b_IMU[2] - dae['ddelta']*e33_IMU) - e32_IMU*(w_bn_b_IMU[1] - dae['ddelta']*e23_IMU) + #dRexp_IMU[1,1] = e32_IMU*(w_bn_b_IMU[0] - dae['ddelta']*e13_IMU) - e12_IMU*(w_bn_b_IMU[2] - dae['ddelta']*e33_IMU) + #dRexp_IMU[1,2] = e12_IMU*(w_bn_b_IMU[1] - dae['ddelta']*e23_IMU) - e22_IMU*(w_bn_b_IMU[0] - dae['ddelta']*e13_IMU) + #dRexp_IMU[2,0] = e23_IMU*w_bn_b_IMU[2] - e33_IMU*w_bn_b_IMU[1] + #dRexp_IMU[2,1] = e33_IMU*w_bn_b_IMU[0] - e13_IMU*w_bn_b_IMU[2] + #dRexp_IMU[2,2] = e13_IMU*w_bn_b_IMU[1] - e23_IMU*w_bn_b_IMU[0] + #if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True: + #RotPole = 0.5 + #else: + #RotPole = 0.0 + #Rst_IMU = RotPole*C.mul( dae['R_c2b_IMU'], (C.inv(C.mul(dae['R_c2b_IMU'].T,dae['R_c2b_IMU'])) - numpy.eye(3)) ) + #ode_R_IMU = C.veccat([dae.ddt(name) for name in ["e11_IMU","e12_IMU","e13_IMU", + #"e21_IMU","e22_IMU","e23_IMU", + #"e31_IMU","e32_IMU","e33_IMU"]]) - ( dRexp_IMU.T.reshape([9,1]) + Rst_IMU.reshape([9,1]) ) + #ode = C.veccat([ode,ode_R_IMU]) + #dae['ConstR1_IMU'] = e11_IMU*e11_IMU + e12_IMU*e12_IMU + e13_IMU*e13_IMU - 1 + #dae['ConstR2_IMU'] = e11_IMU*e21_IMU + e12_IMU*e22_IMU + e13_IMU*e23_IMU + #dae['ConstR3_IMU'] = e11_IMU*e31_IMU + e12_IMU*e32_IMU + e13_IMU*e33_IMU + #dae['ConstR4_IMU'] = e21_IMU*e21_IMU + e22_IMU*e22_IMU + e23_IMU*e23_IMU - 1 + #dae['ConstR5_IMU'] = e21_IMU*e31_IMU + e22_IMU*e32_IMU + e23_IMU*e33_IMU + #dae['ConstR6_IMU'] = e31_IMU*e31_IMU + e32_IMU*e32_IMU + e33_IMU*e33_IMU - 1#}}} +#}}} + + if 'kinematicIMUAccelerationModel' in conf and conf['kinematicIMUAccelerationModel']:#{{{ #dddelta = xDotSol['ddelta'] # TODO effect of pIMU and dddelta dddelta = 0.0 @@ -565,6 +684,7 @@ def carouselModel(conf): ode = C.veccat([ode,dae.ddt('dx_IMU')-ddp_IMU[0], dae.ddt('dy_IMU')-ddp_IMU[1], dae.ddt('dz_IMU')-ddp_IMU[2]]) +#}}} if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True: cPole = 0.5 From 1afc09f6d2cfec63f1b4084a41c8d38618324460 Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Mon, 2 Jun 2014 17:18:27 +0200 Subject: [PATCH 25/27] Added jacobian of constraints as member of ocprt --- rawe/ocp/ocg_interface.py | 3 +++ rawe/ocp/ocprt.py | 4 +++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/rawe/ocp/ocg_interface.py b/rawe/ocp/ocg_interface.py index fa230c5..f691729 100644 --- a/rawe/ocp/ocg_interface.py +++ b/rawe/ocp/ocg_interface.py @@ -124,6 +124,9 @@ return memcpyMat(val, acadoVariables.sigmaN, nr, nc, ACADO_NX, ACADO_NX); } #endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */ +int py_get_A(real_t * val, const int nr, const int nc){ + return memcpyMat(val, acadoWorkspace.A, nr, nc, 100, ACADO_NX + ACADO_NU*ACADO_N); } + #if ACADO_USE_ARRIVAL_COST == 1 int py_set_SAC(real_t * val, const int nr, const int nc){ return memcpyMat(acadoVariables.SAC, val, nr, nc, ACADO_NX, ACADO_NX); } diff --git a/rawe/ocp/ocprt.py b/rawe/ocp/ocprt.py index 3fa364d..e3dfe8e 100644 --- a/rawe/ocp/ocprt.py +++ b/rawe/ocp/ocprt.py @@ -52,7 +52,7 @@ def blah(self,*args,**kwargs): return blah class OcpRT(object): - _canonicalNames = ['x','u','z','y','yN','x0','S','SN','SAC','xAC','WL','sigmaN'] + _canonicalNames = ['x','u','z','y','yN','x0','S','SN','SAC','xAC','WL','sigmaN','A'] @property def ocp(self): @@ -101,6 +101,7 @@ def __init__(self, ocp, if ocpOptions["CG_COMPUTE_COVARIANCE_MATRIX"]: self.sigmaN = numpy.zeros((self._lib.py_get_ACADO_NX(), self._lib.py_get_ACADO_NX())) + self.A = numpy.zeros((100,self._lib.py_get_ACADO_NX() + self._lib.py_get_ACADO_NU()*self._lib.py_get_ACADO_N() )) if self._lib.py_get_ACADO_NXA() > 0: self.z = numpy.zeros((self._lib.py_get_ACADO_N(), self._lib.py_get_ACADO_NXA())) @@ -224,6 +225,7 @@ def _getAll(self): self._callMat(self._lib.py_get_SN, self.SN) if self._lib.py_get_ACADO_COMPUTE_COVARIANCE_MATRIX(): self._callMat(self._lib.py_get_sigmaN, self.sigmaN) + self._callMat(self._lib.py_get_A, self.A) if self._lib.py_get_ACADO_INITIAL_STATE_FIXED(): self._callMat(self._lib.py_get_x0, self.x0) if self._lib.py_get_ACADO_USE_ARRIVAL_COST(): From c7df32ff4ed7371cae3209076f55bd5d573026c0 Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Tue, 3 Jun 2014 17:17:47 +0200 Subject: [PATCH 26/27] Polynomial order can now be set seperately for omega and acceleration --- rawe/models/carousel.py | 64 +++++++++-------------------------------- 1 file changed, 14 insertions(+), 50 deletions(-) diff --git a/rawe/models/carousel.py b/rawe/models/carousel.py index dd51ec6..cb4e01d 100644 --- a/rawe/models/carousel.py +++ b/rawe/models/carousel.py @@ -515,9 +515,9 @@ def carouselModel(conf): if 'polynomialApproximationForIMUAngularVelocity' in conf and conf['polynomialApproximationForIMUAngularVelocity']:#{{{ #Add the control inputs for the polynomial coefficients. That's all we have to do here. The rest is taken care of by the constraints in the MHE formulation - Npoly = conf['PolynomialOrder'] - polynomialCoefficients_AngularVelocity = C.SXMatrix(3,Npoly+1) - for k in range(Npoly+1): + Npoly_omega = conf['PolynomialOrder_omega'] + polynomialCoefficients_AngularVelocity = C.SXMatrix(3,Npoly_omega+1) + for k in range(Npoly_omega+1): polynomialCoefficients_AngularVelocity[0,k] = dae.addU('polynomialCoefficient_AngularVelocity_x_'+str(k)) polynomialCoefficients_AngularVelocity[1,k] = dae.addU('polynomialCoefficient_AngularVelocity_y_'+str(k)) polynomialCoefficients_AngularVelocity[2,k] = dae.addU('polynomialCoefficient_AngularVelocity_z_'+str(k)) @@ -530,15 +530,15 @@ def carouselModel(conf): Nimu = conf['NumberOfIMUMeasurementsBetweenAbsoluteMeasurements'] m = Nimu-1 tt = C.ssym('tt') - gram = C.SXMatrix(Npoly+1,1) + gram = C.SXMatrix(Npoly_omega+1,1) gram[0,0] = 1.0/C.sqrt(m+1.0) - if Npoly>0: + if Npoly_omega>0: #Also do the linear function manually, cause it relies on gammanm = 1/infinity = 0, but which is not allowed n=0.0 alphanm = m/(n+1.0)*C.sqrt((4.0*(n+1.0)**2-1.0)/((m+1.0)**2-(n+1.0)**2)) gram[1,0] = alphanm*tt*gram[0,0] gammanm = 0.0 - for k in range(2,Npoly+1): + for k in range(2,Npoly_omega+1): n = k-1.0 alphanm = m/(n+1.0)*C.sqrt((4.0*(n+1.0)**2-1.0)/((m+1.0)**2-(n+1.0)**2)) n = n-1.0 @@ -562,7 +562,7 @@ def carouselModel(conf): w_bn_imu_end_x = 0.0 w_bn_imu_end_y = 0.0 w_bn_imu_end_z = 0.0 - for k in range(Npoly+1): + for k in range(Npoly_omega+1): w_bn_imu_start_x += dae['polynomialCoefficient_AngularVelocity_x_'+str(k)]*polyEvalStart[k] w_bn_imu_start_y += dae['polynomialCoefficient_AngularVelocity_y_'+str(k)]*polyEvalStart[k] w_bn_imu_start_z += dae['polynomialCoefficient_AngularVelocity_z_'+str(k)]*polyEvalStart[k] @@ -593,42 +593,6 @@ def carouselModel(conf): dae['Output2'] = dae['w_bn_b_y'] - dae['BodyAngularVelocity_dummy_y'] dae['Output3'] = dae['w_bn_b_z'] - dae['BodyAngularVelocity_dummy_z'] - #if 0:#{{{ - #assert conf['polynomialApproximationForIMUAcceleration'], 'At the moment, only polynomials for angular velocity are not supported. Please do proper adding of t and delta_t, and create the polynomials (gram) here if you want to do it.' - ## Built the expression for the IMU Angular Velocity - #IMUAngularVelocity = C.SXMatrix(3,1) - #for k in range(Npoly+1): - #IMUAngularVelocity += polynomialCoefficients_AngularVelocity[:,k]*gram[k,0] - #w_bn_b_IMU = C.mul(conf['RIMU'].T, IMUAngularVelocity) - #[e11_IMU,e12_IMU,e13_IMU,e21_IMU,e22_IMU,e23_IMU,e31_IMU,e32_IMU,e33_IMU] = dae.addX(['e11_IMU','e12_IMU','e13_IMU','e21_IMU','e22_IMU','e23_IMU','e31_IMU','e32_IMU','e33_IMU']) - #dae['R_c2b_IMU'] = C.blockcat([[e11_IMU,e12_IMU,e13_IMU], - #[e21_IMU,e22_IMU,e23_IMU], - #[e31_IMU,e32_IMU,e33_IMU]]) - #dRexp_IMU = C.SXMatrix(3,3) - #dRexp_IMU[0,0] = e21_IMU*(w_bn_b_IMU[2] - dae['ddelta']*e33_IMU) - e31_IMU*(w_bn_b_IMU[1] - dae['ddelta']*e23_IMU) - #dRexp_IMU[0,1] = e31_IMU*(w_bn_b_IMU[0] - dae['ddelta']*e13_IMU) - e11_IMU*(w_bn_b_IMU[2] - dae['ddelta']*e33_IMU) - #dRexp_IMU[0,2] = e11_IMU*(w_bn_b_IMU[1] - dae['ddelta']*e23_IMU) - e21_IMU*(w_bn_b_IMU[0] - dae['ddelta']*e13_IMU) - #dRexp_IMU[1,0] = e22_IMU*(w_bn_b_IMU[2] - dae['ddelta']*e33_IMU) - e32_IMU*(w_bn_b_IMU[1] - dae['ddelta']*e23_IMU) - #dRexp_IMU[1,1] = e32_IMU*(w_bn_b_IMU[0] - dae['ddelta']*e13_IMU) - e12_IMU*(w_bn_b_IMU[2] - dae['ddelta']*e33_IMU) - #dRexp_IMU[1,2] = e12_IMU*(w_bn_b_IMU[1] - dae['ddelta']*e23_IMU) - e22_IMU*(w_bn_b_IMU[0] - dae['ddelta']*e13_IMU) - #dRexp_IMU[2,0] = e23_IMU*w_bn_b_IMU[2] - e33_IMU*w_bn_b_IMU[1] - #dRexp_IMU[2,1] = e33_IMU*w_bn_b_IMU[0] - e13_IMU*w_bn_b_IMU[2] - #dRexp_IMU[2,2] = e13_IMU*w_bn_b_IMU[1] - e23_IMU*w_bn_b_IMU[0] - #if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True: - #RotPole = 0.5 - #else: - #RotPole = 0.0 - #Rst_IMU = RotPole*C.mul( dae['R_c2b_IMU'], (C.inv(C.mul(dae['R_c2b_IMU'].T,dae['R_c2b_IMU'])) - numpy.eye(3)) ) - #ode_R_IMU = C.veccat([dae.ddt(name) for name in ["e11_IMU","e12_IMU","e13_IMU", - #"e21_IMU","e22_IMU","e23_IMU", - #"e31_IMU","e32_IMU","e33_IMU"]]) - ( dRexp_IMU.T.reshape([9,1]) + Rst_IMU.reshape([9,1]) ) - #ode = C.veccat([ode,ode_R_IMU]) - #dae['ConstR1_IMU'] = e11_IMU*e11_IMU + e12_IMU*e12_IMU + e13_IMU*e13_IMU - 1 - #dae['ConstR2_IMU'] = e11_IMU*e21_IMU + e12_IMU*e22_IMU + e13_IMU*e23_IMU - #dae['ConstR3_IMU'] = e11_IMU*e31_IMU + e12_IMU*e32_IMU + e13_IMU*e33_IMU - #dae['ConstR4_IMU'] = e21_IMU*e21_IMU + e22_IMU*e22_IMU + e23_IMU*e23_IMU - 1 - #dae['ConstR5_IMU'] = e21_IMU*e31_IMU + e22_IMU*e32_IMU + e23_IMU*e33_IMU - #dae['ConstR6_IMU'] = e31_IMU*e31_IMU + e32_IMU*e32_IMU + e33_IMU*e33_IMU - 1#}}} #}}} if 'kinematicIMUAccelerationModel' in conf and conf['kinematicIMUAccelerationModel']:#{{{ @@ -645,17 +609,17 @@ def carouselModel(conf): delta_t = dae.addU('delta_t') t_interval = t-delta_t Nimu = conf['NumberOfIMUMeasurementsBetweenAbsoluteMeasurements'] - Npoly = conf['PolynomialOrder'] + Npoly_acceleration = conf['PolynomialOrder_acceleration'] m = Nimu-1 - gram = C.SXMatrix(Npoly+1,1) + gram = C.SXMatrix(Npoly_acceleration+1,1) gram[0,0] = 1.0/C.sqrt(m+1.0) - if Npoly>0: + if Npoly_acceleration>0: #Also do the linear function manually, cause it relies on gammanm = 1/infinity = 0, but which is not allowed n=0.0 alphanm = m/(n+1.0)*C.sqrt((4.0*(n+1.0)**2-1.0)/((m+1.0)**2-(n+1.0)**2)) gram[1,0] = alphanm*t_interval*gram[0,0] gammanm = 0.0 - for k in range(2,Npoly+1): + for k in range(2,Npoly_acceleration+1): n = k-1.0 alphanm = m/(n+1.0)*C.sqrt((4.0*(n+1.0)**2-1.0)/((m+1.0)**2-(n+1.0)**2)) n = n-1.0 @@ -663,14 +627,14 @@ def carouselModel(conf): gammanm = alphanm/alphanmm1 gram[k,0] = alphanm*t_interval*gram[k-1,0]-gammanm*gram[k-2,0] #Add the control inputs for the polynomial coefficients - polynomialCoefficients = C.SXMatrix(3,Npoly+1) - for k in range(Npoly+1): + polynomialCoefficients = C.SXMatrix(3,Npoly_acceleration+1) + for k in range(Npoly_acceleration+1): polynomialCoefficients[0,k] = dae.addU('polynomialCoefficient_Acceleration_x_'+str(k)) polynomialCoefficients[1,k] = dae.addU('polynomialCoefficient_Acceleration_y_'+str(k)) polynomialCoefficients[2,k] = dae.addU('polynomialCoefficient_Acceleration_z_'+str(k)) # Built the expression for the IMU Acceleration IMUAcceleration = C.SXMatrix(3,1) - for k in range(Npoly+1): + for k in range(Npoly_acceleration+1): IMUAcceleration += polynomialCoefficients[:,k]*gram[k,0] else: IMUAcceleration = C.vertcat([dae['IMUAcceleration1'], dae['IMUAcceleration2'], dae['IMUAcceleration3']]) From 50eebb6a83c846795d7ce0ad464de35f9cbab87b Mon Sep 17 00:00:00 2001 From: Kurt Geebelen Date: Wed, 4 Jun 2014 10:00:33 +0200 Subject: [PATCH 27/27] using matplotlib with Agg so that we don't need an X server --- rawe/sim.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rawe/sim.py b/rawe/sim.py index d016e6b..2a95c0e 100644 --- a/rawe/sim.py +++ b/rawe/sim.py @@ -18,6 +18,8 @@ import time import casadi as C import numpy +import matplotlib as mpl +mpl.use('Agg') import matplotlib.pyplot as plt def getitemMsg(d,name,msg):