ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
oderk4.h
Go to the documentation of this file.
1 /********************************************************************************
2  * Copyright (C) 2017-2020 German Aerospace Center (DLR).
3  * Eclipse ADORe, Automated Driving Open Research https://eclipse.org/adore
4  *
5  * This program and the accompanying materials are made available under the
6  * terms of the Eclipse Public License 2.0 which is available at
7  * http://www.eclipse.org/legal/epl-2.0.
8  *
9  * SPDX-License-Identifier: EPL-2.0
10  *
11  * Contributors:
12  * Daniel Heß - initial API and implementation
13  ********************************************************************************/
14 
15 #pragma once
16 
17 #include "aodesolver.h"
18 #include "adoremath.h"
19 
20 namespace adore
21 {
22  namespace mad
23  {
27  template<typename T>
28  class OdeRK4 : public AOdeSolver<T>
29  {
30  public:
31  OdeRK4() {
32  }
33 
34  virtual ~OdeRK4() {
35  }
36 
37  virtual adoreMatrix<T> solve(AOdeModel<T>* model, const adoreMatrix<double, 1, 0>& time, const adoreMatrix<double, 0, 1>& x0) override
38  {
39  int n = x0.nr();
40  int m = time.nc();
41  adoreMatrix<T> result(n, m);
42  adoreMatrix<T, 0, 1> k1(n, 1);
43  adoreMatrix<T, 0, 1> k2(n, 1);
44  adoreMatrix<T, 0, 1> k3(n, 1);
45  adoreMatrix<T, 0, 1> k4(n, 1);
46  result.set_size(n, m);
47  set_colm(result, 0) = x0;
48  for (int i = 1; i < m; i++)
49  {
50  T dt = time(i) - time(i - 1);
51  model->f(time(i - 1), colm(result, i - 1), k1);
52  model->f(time(i - 1) + dt / (T)2, colm(result, i - 1) + (dt / (T)2)*k1, k2);
53  model->f(time(i - 1) + dt / (T)2, colm(result, i - 1) + (dt / (T)2)*k2, k3);
54  model->f(time(i - 1) + dt, colm(result, i - 1) + dt*k3, k4);
55  set_colm(result, i) = colm(result, i - 1) + dt / (T)6 * (k1 + k2*(T)2 + k3*(T)2 + k4);
56  }
57  return result;
58  }
59  virtual adoreMatrix<T> solve_with_output(AOdeModelWithOutput<T>* model, const adoreMatrix<double, 1, 0>& time, const adoreMatrix<double, 0, 1>& x0, adoreMatrix<double>& Y_out) override
60  {
61  int n = x0.nr();
62  int m = time.nc();
63  adoreMatrix<T> result(n, m);
64  adoreMatrix<T, 0, 1> k1(n, 1);
65  adoreMatrix<T, 0, 1> k2(n, 1);
66  adoreMatrix<T, 0, 1> k3(n, 1);
67  adoreMatrix<T, 0, 1> k4(n, 1);
68  adoreMatrix<T, 0, 1> y(Y_out.nr(),1);
69  result.set_size(n, m);
70  set_colm(result, 0) = x0;
71  for (int i = 1; i < m; i++)
72  {
73  T dt = time(i) - time(i - 1);
74  model->fh(time(i - 1), colm(result, i - 1), k1, y);//only the output of time(i-1) is stored
75  set_colm(Y_out, i - 1) = y;
76  model->fh(time(i - 1) + dt / (T)2, colm(result, i - 1) + (dt / (T)2)*k1, k2,y);
77  model->fh(time(i - 1) + dt / (T)2, colm(result, i - 1) + (dt / (T)2)*k2, k3,y);
78  model->fh(time(i - 1) + dt, colm(result, i - 1) + (dt / (T)2)*k1, k4,y);
79  set_colm(result, i) = colm(result, i - 1) + dt / (T)6 * (k1 + k2*(T)2 + k3*(T)2 + k4);
80  }
81 
82  model->fh(time(m - 1), colm(result, m - 1), k1, y);//evaluate fh for last point to get output
83  set_colm(Y_out, m - 1) = y;
84  return result;
85 
86  }
87  };
88  }
89 }
Definition: aodemodel.h:53
virtual void fh(T t, const adoreMatrix< T, 0, 1 > &x_in, adoreMatrix< T, 0, 1 > &dx_out, adoreMatrix< T, 0, 1 > &y_out)=0
Definition: aodemodel.h:30
virtual void f(T t, const adoreMatrix< T, 0, 1 > &x_in, adoreMatrix< T, 0, 1 > &dx_out)=0
Definition: aodesolver.h:28
Definition: oderk4.h:29
virtual adoreMatrix< T > solve(AOdeModel< T > *model, const adoreMatrix< double, 1, 0 > &time, const adoreMatrix< double, 0, 1 > &x0) override
Definition: oderk4.h:37
OdeRK4()
Definition: oderk4.h:31
virtual ~OdeRK4()
Definition: oderk4.h:34
virtual adoreMatrix< T > solve_with_output(AOdeModelWithOutput< T > *model, const adoreMatrix< double, 1, 0 > &time, const adoreMatrix< double, 0, 1 > &x0, adoreMatrix< double > &Y_out) override
Definition: oderk4.h:59
x0
Definition: adore_set_goal.py:25
y
Definition: adore_set_goal.py:31
Definition: areaofeffectconverter.h:20