[882] | 1 | /* -------------------------------------------------------------------------
|
---|
| 2 | * BKG NTRIP Server
|
---|
| 3 | * -------------------------------------------------------------------------
|
---|
| 4 | *
|
---|
| 5 | * Function: rungeKutta4
|
---|
| 6 | *
|
---|
| 7 | * Purpose: Fourth order Runge-Kutta numerical integrator for ODEs
|
---|
| 8 | *
|
---|
| 9 | * Example usage - numerical integration dy/dt = cos(x) :
|
---|
| 10 | *
|
---|
| 11 | * std::vector<double> dydtcos(double x, vector<double> y) {
|
---|
| 12 | * std::vector<double> dydt(y)
|
---|
| 13 | * dydt.at(i)=cos(x);
|
---|
| 14 | * return dydt;
|
---|
| 15 | * }
|
---|
| 16 | *
|
---|
| 17 | * vector<double> yi(1,0.0);
|
---|
| 18 | * double x=0.0, dx=0.10;
|
---|
| 19 | *
|
---|
| 20 | * while (x<10.0) {
|
---|
| 21 | * yi=rungeKutta4(yi,x,dx,(*dydtcos));
|
---|
| 22 | * x+=dx;
|
---|
| 23 | * }
|
---|
| 24 | *
|
---|
| 25 | * Author: L. Mervart
|
---|
| 26 | *
|
---|
| 27 | * Created: 07-Mai-2008
|
---|
| 28 | *
|
---|
| 29 | * Changes:
|
---|
| 30 | *
|
---|
| 31 | /******************************************************************************/
|
---|
| 32 |
|
---|
| 33 | #include "rungekutta4.h"
|
---|
| 34 |
|
---|
| 35 | using namespace std;
|
---|
| 36 |
|
---|
| 37 | vector<double> rungeKutta4(
|
---|
| 38 | double xi, // the initial x-value
|
---|
| 39 | vector<double> yi, // vector of the initial y-values
|
---|
| 40 | double dx, // the step size for the integration
|
---|
| 41 | vector<double> (*derivatives)(double x, vector<double> y) ) {
|
---|
| 42 | // a pointer to a function that returns the derivative
|
---|
| 43 | // of a function at a point (x,y), where y is an STL
|
---|
| 44 | // vector of elements. Returns a vector of the same
|
---|
| 45 | // size as y.
|
---|
| 46 |
|
---|
| 47 | // total number of elements in the vector
|
---|
| 48 | int n=yi.size();
|
---|
| 49 |
|
---|
| 50 | // first step
|
---|
| 51 | vector<double> k1;
|
---|
| 52 | k1=derivatives(xi, yi);
|
---|
| 53 | for (int i=0; i<n; ++i) {
|
---|
| 54 | k1.at(i)*=dx;
|
---|
| 55 | }
|
---|
| 56 |
|
---|
| 57 | // second step
|
---|
| 58 | vector<double> k2(yi);
|
---|
| 59 | for (int i=0; i<n; ++i) {
|
---|
| 60 | k2.at(i)+=k1.at(i)/2.0;
|
---|
| 61 | }
|
---|
| 62 | k2=derivatives(xi+dx/2.0,k2);
|
---|
| 63 | for (int i=0; i<n; ++i) {
|
---|
| 64 | k2.at(i)*=dx;
|
---|
| 65 | }
|
---|
| 66 |
|
---|
| 67 | // third step
|
---|
| 68 | vector<double> k3(yi);
|
---|
| 69 | for (int i=0; i<n; ++i) {
|
---|
| 70 | k3.at(i)+=k2.at(i)/2.0;
|
---|
| 71 | }
|
---|
| 72 | k3=derivatives(xi+dx/2.0,k3);
|
---|
| 73 | for (int i=0; i<n; ++i) {
|
---|
| 74 | k3.at(i)*=dx;
|
---|
| 75 | }
|
---|
| 76 |
|
---|
| 77 | // fourth step
|
---|
| 78 | vector<double> k4(yi);
|
---|
| 79 | for (int i=0; i<n; ++i) {
|
---|
| 80 | k4.at(i)+=k3.at(i);
|
---|
| 81 | }
|
---|
| 82 | k4=derivatives(xi+dx,k4);
|
---|
| 83 | for (int i=0; i<n; ++i) {
|
---|
| 84 | k4.at(i)*=dx;
|
---|
| 85 | }
|
---|
| 86 |
|
---|
| 87 | // sum the weighted steps into yf and return the final y values
|
---|
| 88 | vector<double> yf(yi);
|
---|
| 89 | for (int i=0; i<n; ++i) {
|
---|
| 90 | yf.at(i)+=(k1.at(i)/6.0)+(k2.at(i)/3.0)+(k3.at(i)/3.0)+(k4.at(i)/6.0);
|
---|
| 91 | }
|
---|
| 92 |
|
---|
| 93 | return yf;
|
---|
| 94 | }
|
---|