1 #include "MUQ/Modeling/LinearSDE.h"
2
3 #include <random>
4
5 #include <unsupported/Eigen/MatrixFunctions>
6
7 #include "MUQ/Utilities/RandomGenerator.h"
8 #include "MUQ/Utilities/Exceptions.h"
9
10 #include "MUQ/Modeling/LinearAlgebra/BlockDiagonalOperator.h"
11
12 using namespace muq::Modeling;
13 using namespace muq::Utilities;
14
LinearSDE(std::shared_ptr<LinearOperator> Fin,std::shared_ptr<LinearOperator> Lin,Eigen::MatrixXd const & Qin,boost::property_tree::ptree options)15 LinearSDE::LinearSDE(std::shared_ptr<LinearOperator> Fin,
16 std::shared_ptr<LinearOperator> Lin,
17 Eigen::MatrixXd const& Qin,
18 boost::property_tree::ptree options) : stateDim(Fin->rows()), F(Fin), L(Lin), Q(Qin)
19 {
20 if(F->rows() != F->cols())
21 {
22 throw muq::WrongSizeError("The system transition matrix, F, must be square, but F has " + std::to_string(F->rows()) + " rows and " + std::to_string(F->cols()) + " columns.");
23 }
24
25 if(F->rows() != L->rows())
26 {
27 throw muq::WrongSizeError("F and L must have the same number of rows, but F has " + std::to_string(F->rows()) + " rows and L has " + std::to_string(L->rows()) + " rows.");
28 }
29
30 // Extract options from the ptree
31 ExtractOptions(options);
32
33 // Compute the Cholesky decomposition of the white noise process
34 sqrtQ = Q.llt().matrixL();
35 };
36
ExtractOptions(boost::property_tree::ptree options)37 void LinearSDE::ExtractOptions(boost::property_tree::ptree options)
38 {
39 dt = options.get("SDE.dt",1e-4);
40 }
41
EvolveState(Eigen::VectorXd const & f0,double T) const42 Eigen::VectorXd LinearSDE::EvolveState(Eigen::VectorXd const& f0,
43 double T) const
44 {
45 Eigen::VectorXd f = f0;
46
47 const int numTimes = std::ceil(T/dt);
48
49 Eigen::VectorXd z;
50
51 // Take all but the last step. The last step might be a partial step
52 for(int i=0; i<numTimes-1; ++i)
53 {
54 z = sqrt(dt) * (sqrtQ.triangularView<Eigen::Lower>() * RandomGenerator::GetNormal(sqrtQ.cols()) ).eval();
55
56 f += dt*F->Apply(f) + L->Apply( z );
57 }
58
59 // Now take the last step
60 double lastDt = T-(numTimes-1)*dt;
61
62 z = sqrt(lastDt) * (sqrtQ.triangularView<Eigen::Lower>() * RandomGenerator::GetNormal(sqrtQ.cols())).eval();
63
64 f += lastDt*F->Apply(f) + L->Apply( z );
65
66 return f;
67 }
68
69
EvolveDistribution(Eigen::VectorXd const & mu0,Eigen::MatrixXd const & gamma0,double T) const70 std::pair<Eigen::VectorXd, Eigen::MatrixXd> LinearSDE::EvolveDistribution(Eigen::VectorXd const& mu0,
71 Eigen::MatrixXd const& gamma0,
72 double T) const
73 {
74
75 Eigen::VectorXd mu = mu0;
76 Eigen::MatrixXd gamma = gamma0;
77
78 const int numTimes = std::ceil(T/dt);
79
80 Eigen::MatrixXd LQLT = L->Apply( L->Apply(Q).transpose().eval() );
81 LQLT = 0.5*(LQLT + LQLT.transpose()); // <- Make sure LQLT is symmetric
82
83 Eigen::MatrixXd Fgamma, k1, k2, k3, k4;
84
85 // Take all but the last step because the last step might be a partial step.
86 for(int i=0; i<numTimes-1; ++i)
87 {
88 k1 = F->Apply(mu);
89 k2 = F->Apply(mu + 0.5*dt*k1);
90 k3 = F->Apply(mu + 0.5*dt*k2);
91 k4 = F->Apply(mu + dt*k3);
92 mu = mu + (dt/6.0)*(k1 + 2.0*k2 + 2.0*k3 + k4);
93
94 Fgamma = F->Apply(gamma);
95 k1 = Fgamma + Fgamma.transpose() + LQLT;
96 Fgamma = F->Apply(gamma + 0.5*dt*k1);
97 k2 = Fgamma + Fgamma.transpose() + LQLT;
98 Fgamma = F->Apply(gamma + 0.5*dt*k2);
99 k3 = Fgamma + Fgamma.transpose() + LQLT;
100 Fgamma = F->Apply(gamma + dt*k3);
101 k4 = Fgamma + Fgamma.transpose() + LQLT;
102
103 gamma = gamma + (dt/6.0)*(k1 + 2.0*k2 + 2.0*k3 + k4);
104 }
105
106 // Take the last step
107 double lastDt = T-(numTimes-1)*dt;
108
109 k1 = F->Apply(mu);
110 k2 = F->Apply(mu + 0.5*lastDt*k1);
111 k3 = F->Apply(mu + 0.5*lastDt*k2);
112 k4 = F->Apply(mu + lastDt*k3);
113 mu = mu + (lastDt/6.0)*(k1 + 2.0*k2 + 2.0*k3 + k4);
114
115 Fgamma = F->Apply(gamma);
116 k1 = Fgamma + Fgamma.transpose() + LQLT;
117 Fgamma = F->Apply(gamma + 0.5*lastDt*k1);
118 k2 = Fgamma + Fgamma.transpose() + LQLT;
119 Fgamma = F->Apply(gamma + 0.5*lastDt*k2);
120 k3 = Fgamma + Fgamma.transpose() + LQLT;
121 Fgamma = F->Apply(gamma + lastDt*k3);
122 k4 = Fgamma + Fgamma.transpose() + LQLT;
123
124 gamma = gamma + (lastDt/6.0)*(k1 + 2.0*k2 + 2.0*k3 + k4);
125
126 return std::make_pair(mu,gamma);
127 }
128
129
Concatenate(std::vector<std::shared_ptr<LinearSDE>> const & sdes,boost::property_tree::ptree options)130 std::shared_ptr<LinearSDE> LinearSDE::Concatenate(std::vector<std::shared_ptr<LinearSDE>> const& sdes,
131 boost::property_tree::ptree options)
132 {
133
134 int stateDim = 0;
135 int stochDim = 0;
136 for(auto& sde : sdes){
137 stateDim += sde->stateDim;
138 stochDim += sde->L->cols();
139 }
140
141 std::vector<std::shared_ptr<LinearOperator>> Fs(sdes.size());
142 std::vector<std::shared_ptr<LinearOperator>> Ls(sdes.size());
143
144 Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(stochDim, stochDim);
145
146 int currDim = 0;
147 for(int i=0; i<sdes.size(); ++i){
148 Fs.at(i) = sdes.at(i)->GetF();
149 Ls.at(i) = sdes.at(i)->GetL();
150
151 Q.block(currDim, currDim, Ls.at(i)->cols(), Ls.at(i)->cols()) = sdes.at(i)->GetQ();
152
153 currDim += Ls.at(i)->cols();
154 }
155
156 auto F = std::make_shared<BlockDiagonalOperator>(Fs);
157 auto L = std::make_shared<BlockDiagonalOperator>(Ls);
158
159 return std::make_shared<LinearSDE>(F,L,Q, options);
160 }
161
162
Discretize(double deltaT)163 std::pair<Eigen::MatrixXd, Eigen::MatrixXd> LinearSDE::Discretize(double deltaT)
164 {
165 Eigen::MatrixXd A = Eigen::MatrixXd::Identity(stateDim,stateDim);
166 Eigen::MatrixXd gamma = Eigen::MatrixXd::Zero(stateDim,stateDim);
167
168 const int numTimes = std::ceil(deltaT/dt);
169
170 Eigen::MatrixXd LQLT = L->Apply( L->Apply(Q).transpose().eval() );
171 LQLT = 0.5*(LQLT + LQLT.transpose()); // <- Make sure LQLT is symmetric
172
173 Eigen::MatrixXd Fgamma, k1, k2, k3, k4;
174
175 // Take all but the last step because the last step might be a partial step.
176 for(int i=0; i<numTimes-1; ++i)
177 {
178 k1 = F->Apply(A);
179 k2 = F->Apply(A + 0.5*dt*k1);
180 k3 = F->Apply(A + 0.5*dt*k2);
181 k4 = F->Apply(A + dt*k3);
182 A = A + (dt/6.0) * (k1 + 2.0*k2 + 2.0*k3 + k4);
183
184 Fgamma = F->Apply(gamma);
185 k1 = Fgamma + Fgamma.transpose() + LQLT;
186 Fgamma = F->Apply(gamma + 0.5*dt*k1);
187 k2 = Fgamma + Fgamma.transpose() + LQLT;
188 Fgamma = F->Apply(gamma + 0.5*dt*k2);
189 k3 = Fgamma + Fgamma.transpose() + LQLT;
190 Fgamma = F->Apply(gamma + dt*k3);
191 k4 = Fgamma + Fgamma.transpose() + LQLT;
192
193 gamma = gamma + (dt/6.0)*(k1 + 2.0*k2 + 2.0*k3 + k4);
194 }
195
196 double lastDt = deltaT-(numTimes-1)*dt;
197 k1 = F->Apply(A);
198 k2 = F->Apply(A + 0.5*lastDt*k1);
199 k3 = F->Apply(A + 0.5*lastDt*k2);
200 k4 = F->Apply(A + lastDt*k3);
201 A = A + (lastDt/6.0) * (k1 + 2.0*k2 + 2.0*k3 + k4);
202
203 Fgamma = F->Apply(gamma);
204 k1 = Fgamma + Fgamma.transpose() + LQLT;
205 Fgamma = F->Apply(gamma + 0.5*lastDt*k1);
206 k2 = Fgamma + Fgamma.transpose() + LQLT;
207 Fgamma = F->Apply(gamma + 0.5*lastDt*k2);
208 k3 = Fgamma + Fgamma.transpose() + LQLT;
209 Fgamma = F->Apply(gamma + lastDt*k3);
210 k4 = Fgamma + Fgamma.transpose() + LQLT;
211
212 gamma = gamma + (lastDt/6.0)*(k1 + 2.0*k2 + 2.0*k3 + k4);
213
214 return std::make_pair(A,gamma);
215
216 }
217