forked from botprof/agv-examples
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathintegration.py
33 lines (27 loc) · 1.05 KB
/
integration.py
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
"""
Python module integration.py for numerical integration routines.
Author: Joshua A. Marshall <[email protected]>
GitHub: https://github.com/botprof/agv-examples
"""
def rk_four(f, x, u, T):
"""
Perform fourth-order Runge-Kutta numerical integration.
The function to integrate is f(x, u, params), where the state variables are
collected in the variable x, we assume a constant input vector u over time
interval T > 0, and params is an array of the system's parameters.
"""
k_1 = f(x, u)
k_2 = f(x + T * k_1 / 2.0, u)
k_3 = f(x + T * k_2 / 2.0, u)
k_4 = f(x + T * k_3, u)
x_new = x + T / 6.0 * (k_1 + 2.0 * k_2 + 2.0 * k_3 + k_4)
return x_new
def euler_int(f, x, u, T):
"""
Perform Euler (trapezoidal) numerical integration.
The function to integrate is f(x, u, params), where the state variables are
collected in the variable x, we assume a constant input vector u over time
interval T > 0, and params is an array of the system's parameters.
"""
x_new = x + T * f(x, u)
return x_new