-
Notifications
You must be signed in to change notification settings - Fork 2
/
RK.cpp
53 lines (42 loc) · 1.62 KB
/
RK.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
#include "RK.h"
#include "MatrixCalc.h"
namespace ADP{
void RK::linear(const std::vector<double>& x, Controllers* controller, inputfun input, const double t)
//const std::vector<double> RK::linear(const std::vector<double>& x, Controllers* controller, inputfun input, const double t)
{
if (controller!=nullptr)
{
std::vector<double> u = controller->input(x,mdt,t);
mu = u;
std::vector<double> k1 (vec(mA * x + mB * u));
u = controller->input(x+k1*mdt/2,0,t+mdt/2);
std::vector<double> k2 (vec(mA * (x+k1*mdt/2) + mB * u));
u = controller->input(x+k2*mdt/2,0,t+mdt/2);
std::vector<double> k3 (vec(mA * (x+k2*mdt/2) + mB * u));
u = controller->input(x+k3*mdt,0,t+mdt);
std::vector<double> k4 (vec(mA * (x+k3*mdt) + mB * u));
mx = std::vector<double>(x+mdt/6*(k1+2*k2+2*k3+k4));
}
else if (input!=nullptr)
{
std::vector<double> u = input(x,mB.size()[0],t);
mu = u;
std::vector<double> k1 (vec(mA * x + mB * u));
u = input(x+k1*mdt/2,mB.size()[0],t+mdt/2);
std::vector<double> k2 (vec(mA * (x+k1*mdt/2) + mB * u));
u = input(x+k2*mdt/2,mB.size()[0],t+mdt/2);
std::vector<double> k3 (vec(mA * (x+k2*mdt/2) + mB * u));
u = input(x+k3*mdt,mB.size()[0],t+mdt);
std::vector<double> k4 (vec(mA * (x+k3*mdt) + mB * u));
mx = std::vector<double>(x+mdt/6*(k1+2*k2+2*k3+k4));
}
else
{
std::vector<double> k1 (vec(mA * x));
std::vector<double> k2 (vec(mA * (x+k1*mdt/2)));
std::vector<double> k3 (vec(mA * (x+k2*mdt/2)));
std::vector<double> k4 (vec(mA * (x+k3*mdt)));
mx = std::vector<double>(x+mdt/6*(k1+2*k2+2*k3+k4));
}
}
}