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