1# 2# This file is part of CasADi. 3# 4# CasADi -- A symbolic framework for dynamic optimization. 5# Copyright (C) 2010-2014 Joel Andersson, Joris Gillis, Moritz Diehl, 6# K.U. Leuven. All rights reserved. 7# Copyright (C) 2011-2014 Greg Horn 8# 9# CasADi is free software; you can redistribute it and/or 10# modify it under the terms of the GNU Lesser General Public 11# License as published by the Free Software Foundation; either 12# version 3 of the License, or (at your option) any later version. 13# 14# CasADi is distributed in the hope that it will be useful, 15# but WITHOUT ANY WARRANTY; without even the implied warranty of 16# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 17# Lesser General Public License for more details. 18# 19# You should have received a copy of the GNU Lesser General Public 20# License along with CasADi; if not, write to the Free Software 21# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 22# 23# 24from pylab import * 25from scipy.linalg import sqrtm 26 27from casadi import * 28from casadi.tools import * 29 30# System states 31states = struct_symSX(["x","y","dx","dy"]) 32x,y,dx,dy = states[...] 33 34# System controls 35controls = struct_symSX(["u","v"]) 36u,v = controls[...] 37 38# System parameters 39parameters = struct_symSX(["k","c","beta"]) 40k,c,beta = parameters[...] 41 42# Provide some numerical values 43parameters_ = parameters() 44parameters_["k"] = 10 45parameters_["beta"] = 1 46parameters_["c"] = 1 47 48vel = vertcat(dx,dy) 49p = vertcat(x,y) 50q = vertcat(u,v) 51 52# System dynamics 53F = -k*(p-q) - beta*v*sqrt(sumsqr(vel) + c**2) 54 55# System right hand side 56rhs = struct_SX(states) 57rhs["x"] = dx 58rhs["y"] = dy 59rhs["dx"] = F[0] 60rhs["dy"] = F[1] 61 62# f = SX.fun("f", controldaeIn(x=states,p=parameters,u=controls),daeOut(ode=rhs)) 63 64 65# # Simulation output grid 66# N = 100 67# tgrid = linspace(0,10.0,N) 68 69# # ControlSimulator will output on each node of the timegrid 70# opts = {} 71# opts["integrator"] = "cvodes" 72# opts["integrator_options"] = {"abstol":1e-10,"reltol":1e-10} 73# csim = ControlSimulator("csim", f, tgrid, opts) 74 75# x0 = states(0) 76 77# # Create input profile 78# controls_ = controls.repeated(csim.getInput("u")) 79# controls_[0,"u"] = 1 # Kick the system with u=1 at the start 80# controls_[N/2,"v"] = 2 # Kick the system with v=2 at half the simulation time 81 82# # Pure simulation 83# csim.setInput(x0,"x0") 84# csim.setInput(parameters_,"p") 85# csim.setInput(controls_,"u") 86# csim.evaluate() 87 88# output = states.repeated(csim.getOutput()) 89 90# # Plot all states 91# for k in states.keys(): 92# plot(tgrid,output[vertcat,:,k]) 93# xlabel("t") 94# legend(tuple(states.keys())) 95 96# print "xf=", output[-1] 97 98# # The remainder of this file deals with methods to calculate the state covariance matrix as it propagates through the system dynamics 99 100# # === Method 1: integrator sensitivity === 101# # PF = d(I)/d(x0) P0 [d(I)/d(x0)]^T 102 103# P0 = states.squared() 104# P0[:,:] = 0.01*DM.eye(states.size) 105# P0["x","dy"] = P0["dy","x"] = 0.002 106 107# # Not supported in current revision, cf. #929 108# # J = csim.jacobian_old(csim.index_in("x0"),csim.index_out("xf")) 109# # J.setInput(x0,"x0") 110# # J.setInput(parameters_,"p") 111# # J.setInput(controls_,"u") 112# # J.evaluate() 113 114# # Jk = states.squared_repeated(J.getOutput()) 115# # F = Jk[-1] 116 117# # PF_method1 = mtimes([F,P0,F.T]) 118 119# # print "State cov (method 1) = ", PF_method1 120 121# # === Method 2: Lyapunov equations === 122# # P' = A.P + P.A^T 123# states_aug = struct_symSX([ 124# entry("orig",sym=states), 125# entry("P",shapestruct=(states,states)) 126# ]) 127 128# A = jacobian(rhs,states) 129 130# rhs_aug = struct_SX(states_aug) 131# rhs_aug["orig"] = rhs 132# rhs_aug["P"] = mtimes(A,states_aug["P"]) + mtimes(states_aug["P"],A.T) 133 134# f_aug = SX.fun("f_aug", controldaeIn(x=states_aug,p=parameters,u=controls),daeOut(ode=rhs_aug)) 135 136# csim_aug = ControlSimulator("csim_aug", f_aug, tgrid, {"integrator":"cvodes"}) 137 138# states_aug(csim_aug.getInput("x0"))["orig"] = x0 139# states_aug(csim_aug.getInput("x0"))["P"] = P0 140 141# csim_aug.setInput(parameters_,"p") 142# csim_aug.setInput(controls_,"u") 143# csim_aug.evaluate() 144 145# output = states_aug.repeated(csim_aug.getOutput()) 146 147# PF_method2 = output[-1,"P"] 148 149# print "State cov (method 2) = ", PF_method2 150 151# # === Method 3: Unscented propagation === 152# # Sample and simulate 2n+1 initial points 153# n = states.size 154 155# W0 = 0 156# x0 = DM(x0) 157# W = DM([ W0 ] + [(1.0-W0)/2/n for j in range(2*n)]) 158 159# sqm = sqrtm(n/(1.0-W0)*DM(P0)).real 160# sample_x = [ x0 ] + [x0+sqm[:,i] for i in range(n)] + [x0-sqm[:,i] for i in range(n)] 161 162# csim.setInput(parameters_,"p") 163# csim.setInput(controls_,"u") 164 165# simulated_x = [] # This can be parallelised 166# for x0_ in sample_x: 167# csim.setInput(x0_,"x0") 168# csim.evaluate() 169# simulated_x.append(csim.getOutput()[-1,:]) 170 171# simulated_x = vertcat(simulated_x).T 172 173# Xf_mean = mtimes(simulated_x,W) 174 175# x_dev = simulated_x-mtimes(Xf_mean,DM.ones(1,2*n+1)) 176 177# PF_method3 = mtimes([x_dev,diag(W),x_dev.T]) 178# print "State cov (method 3) = ", PF_method3 179 180# show() 181