Table of contents

Introduction..2
Model of the system..3
Model of the bump..4
Differential Equation of Motion (2DOF).6
Natural Frequency and Mode Shapes...8
General Solution...19
D.E of Motion using Lagrange Equation20
Differential Equation of Motion (4DOF)..22
Nonlinear Differential Equation of Motion.24
Solution with Laplace Method..25
Numerical Solution..26
Sample of Excel worksheet...31
VBA code for numerical solutions..32

" #

$ %

&

Introduction
In this project, Im trying to model a car which could be a Sedan
car, a truck, or an SUV, when passing through a Bump. This bump
is acting like a base excitation for our system.
First of all we will assume a 2DOF linear system; however, I will
derive the differential equation for 4DOF and Non linear system.

The different parts of this project are:

1. Modeling of the system
2. Modeling of the bump
3. Differential Equation of Motion 2DOF (linear)
4. Calculation of Natural Frequency and Mode shapes
5. Differential Equation of Motion Using Lagrange Eq.
6. Differential Equation of Motion 4DOF (linear)
7. Differential Equation of Motion 2DOF(Non linear)
8. Calculating X(t),(t) with Laplace Method
9. Calculating X(t),(t) with Numerical method Runge Kutta(4)
10. State variables for 2DOF (linear)
11. State variables for 2DOF (non linear)
12. VBA codes of 4-th order Runge Kutta for this problem
13. Graphs of X(t),(t) with some inputs (with Excel)

" #

$ %

&

'

Modeling of the System

Model of a car:

First assumption: The effects of the tires are negligible.

Second assumption: The left side and the right side of the car are symmetric.

Dynamic model:

" #

$ %

&

Modeling of the Bump

First assumption: We assume that the bump is like a half sine wave.
So; the effect of bump is like base excitation on the system: z1, z2.

Top graph: Front wheel

h: height of the bump

d: horizontal coordinate
D: length of the bump
L: distance between the front and back tires

Bottom graph: rear wheel

Second assumption: We assume that the cars speed is constant (acceleration is

zero) at this period.

z1 = h0 . sin

z1 =

.d

h0 . sin

=V .t
d
t p1 =

.d

V : Car Speed

if t < tp1

D
if t tp1

0,
0
z 2 = h0 . sin

.V .t
D

Note: t l =

D
V

if t < t l
if t l < t < tp 2
if t > tp 2

L
L+D
tp 2 =
V
V

" #

$ %

&

Other form of z1, z2 function:

Introduce U c (t ) =

0 tC
1 t >C

z1 = 1 U tp1 (t ) .h0 . sin

.V0 .t
D

z 2 = U tl (t ) U tp2 (t ) .h0 . sin

.V0 .t
D

t < tp1 z1 = (1 0).h0 . sin

t < t L z 2 = (0 0).h0 . sin

.V0 .t
D

.V0 .t
D

Check,
t L t tp 2 z 2 (1 0).h0 . sin
t > tp 2 z 2 = (1 1).h0 . sin

=0

.V0 .t
D

.V0 .t
D

= h0 . sin

.V0 .t
D

=0

" #

$ %

&

Differential Equation of Motion 2DOF (linear)

Assume:

z 2 < x l 2
z1 < x + l 2
..

Fx = m. x
..

Fs 2 Fc 2 Fc1 Fs1 = m x
Fs 2 = k 2 ( x l1 z 2 )
Fs1 = k1 ( x l1 z 2 )

Fc 2 = c 2 ( x l 2 z 2 )
.

Fc1 = c1 ( x l1 z 2 )
.

..

..

..

k 2 x + k 2 l 2 + k 2 z 2 c 2 x + c 2 l 2 + c 2 z 2 k1 x k1l1 + k1 z1 c1 x c1l1 + c1 z 1 = m x
..

m x + (c1 + c 2 ) x + (c1l1 c 2 l 2 ) + (k1 + k 2 ) x + (k1l1 k 2 l 2 ) = c1 z 1 + c 2 z 2 + k1 z1 + k 2 z 2

" #

$ %

&

)
..

McG = I
..

Fs 2 .l 2 + Fc 2 l 2 Fc1 .l1 Fs1l1 = I

.

..

k 2 l 2 ( x l 2 z 2 ) + c 2 l 2 ( x l 2 z 2 ) c1l1 ( x + l1 z 1 ) k1l1 ( x + l1 z1 ) = I
..

I + (c 2 l 22 + c1l12 ) + (c1l1 c 2 l 2 ) x + (k 2 l 22 + k1l12 ) + (k1l1 k 2 l 2 ) x = c1l1 z 1 c 2 l 2 z 2 + k1l1 z1 k 2 l 2 z

If we put 1, 2 in matrix format;

m 0
0

..

. x.. +

c1

c1l1 c 2 l 2

c1l1 c 2 l 2

c 2 l 22 + c1l12

. x. +

k1 + k 2

k1l1 k 2 l 2

k1l1 k 2 l 2

k 2 l 22 + k1l12

c1 z 1 + c 2 z 2 + k1 z1 + k 2 z 2
.

c1l1 z 1 c 2 l 2 z 2 + k1l1 z1 k 2 l 2 z 2

" #

$ %

&

Calculation of Natural Frequency and Mode shapes

Assuming Reasonable values for all parameter, we have:

I = m.r 2

r : Radius gyration of the car

Reasonable approximate values for a Sedan car;

r 2 = 0.6m 2

m = 1000kg

k1 = k 2 = 5000 N m

c1 = c 2 = 500 N .s m
l1 = 0.8m

l 2 = 1 .2 m

Reasonable approximate values for a SUV;

r 2 = 0.65m 2

m = 3000kg

k1 = k 2 = 1500 N m

l1 = 1m

c1 = c 2 = 1500 N .s m
l 2 = 1 .5 m

Solved for a bump with h = 0.1m , D = 1.5m

Eigen Value Problem and Mode Shapes

For our model we assume c1 = c2 = c and k1 = k2 = k
eq .3

m 0
0

..

. x.. +

2c
c(l1 l 2 ) x
2k
k (l1 l 2 ) x
g (t )
. . +
.
= 1
2
2
2
2
c(l1 l 2 ) c(l1 + l1 )
k (l1 l 2 ) k (l1 + l1 )
g 2 (t )

Where:

g1 (t ) = h0 1 u tp1 (t ) .(k . sin f t + c , f . cos f t ) + h0 u tL (t ) u tp2 (t ) .(k . sin f t + c , f . cos f t )

g 2 (t ) = h0 .l1 1 u tp1 (t ) .(k . sin f t + c , f . cos f t ) + h0 .l 2 u tL (t ) u tp2 (t ) .(k . sin f t + c , f . cos f t )

Note: f =

.V
D
!

" #

$ %

&

Eigen Value Problem:

([k ] .[m]) = 0
2

det

2k 2

k (l1 l 2 )

k (l1 l 2 ) k (l12 + l 22 ) 2 .I

=0

k 2 (l1 + l 2 ) 2
2k k 2 2
+ (l1 + l 2 ) . 2 +
=0
m I
M .I

2k k 2 2
=
+ (l1 + l 2 )
m I
2

2k k 2 2
+ (l1 + l 2 )
m I

k 2 (l1 + l 2 ) 2
4
m.I

For a sedan car we have:

m = 1000kg , I = m.r 2

and

l1 = 0.8

k1 = k 2 = 5000 N m

l 2 = 1 .2 ,

r 2 = 0.6

I = 600 Nm 2
c1 = c 2 = 500 N .s m

If we plug in eq.(5)

10000 5000
(5000) 2 .(0.8 + 1.2) 2
+
( 0 .8 2 + 1 .2 2 ) 2 +
=0
1000
600
1000 600

4 27.334 2 + 166.677 = 0 \
12 = 9.182
1 = 3.03(rad sec)

Back sub

22 = 18.151
2 = 4.26(rad sec)

in (4)

" #

$ %

&

10000 (9.182).(1000)
5000.(0.8 1.2)
2k 12 m
k (l1 l 2 )
=
2
2
2
5000.(0.8 1.2)
5000.(0.8 2 + 1.2 2 ) (9.182).(600)
k (l1 l 2 ) k (l1 + l 2 ) 1 .I
=

818
2000
2000 4890.8

Sub

1 =

1
( First.Mode)
0.409

in (4)

10000 (18.151).(1000)

5000.(0.8 1.2)

1
4.075

5000.(0.8 1.2)
2

5000.(0.8 + 1.2 ) (18.151).(600)

8151

2000

2000 490.6

( Second .Mode)

Approximate Mode Shapes and Natural Frequencies

For Sedan Cars are,

1 = 3.03 rad sec

1 =

2 = 4.26 rad sec

2 =

1
0.409
1
4.075

" #

$ %

&

For an SUV we have

m = 3000kg I = m / r 2
k1 = k 2 = 15000 N m
c1 = c 2 = 1500 N m

and

r = 0.64 I = 1920 N / m 2
l1 = 1
l 2 = 1 .5

Plug the numbers in (5),

30000 15000 2
(15000) 2 .(1 + 1.5) 2
+
(1 + 1.5 2 . 2 +
=0
3000
1920
(3000).(1920)

4 35.39 2 + 244.14 = 0
12 = 9.39 , 22 = 26

1 = 3.064 rad sec ,

2 = 5.1 rad sec

Back sub 1 in (4),

30000 (9.39).(3000)
15000.(1 1.5)
1830
7500
=
2
15000.(1 1.5)
15000.(1 + 1.5 ) (9.39).(1920)
7500 30721.2

1 =

1
0.244

Back sub

in (4),

" #

$ %

&

30000 (26).(3000)
15000.(1 1.5)
48000 7500
=
2
15000.(1 1.5)
15000.(1 + 1.5 ) (26).(1920)
7500 1170
2 =

1
6.4

Approximate Mode Shapes and Natural Frequencies

For an SUV is:

1 = 3.06 rad sec

1 =

2 = 5.1 rad sec

2 =

1
0.244
1
6 .4

Conclude:

If we vibrate a sedan and a SUV at their second Natural Frequency, the SUV is more
capable of rotating than a sedan. However, at their first Natural Frequency, the situation
is vice versa.

Important Note:

[k ] =

2.k

k .(l1 l 2 )
2
1

2
1

k .(l1 l 2 ) k .(l + l )

=k

l1 l 2

l1 l 2

l12 + l12

" #

$ %

&

'

[C ] =

C.(l1 l 2 )

2.C

2
1

= C.

2
1

C.(l1 l 2 ) C.(l + l )

l1 l 2

l1 l 2

l12 + l12

As we can see above, the [C] matrix is a factor of [k]; so we will be able to calculate
modal C.

Modal Mass: (for sedan car)

1000 0
1
1
, M =
0
600
0.409 4.075

m1
0

0
1100
0
T
= [ ] .[m][
. ] =
m2
0
10963

m1 = 1100kg , m2 = 10963kg

Modal Damping: (sedan)

C1

C2

. ], [C ] =
= [ ] .[C ][

1000

200

C1

200

1090

C2

1010

19900

C1 = 1010 N .s m , C 2 = 19900 N .s m

Modal Stiffness: (sedan)

k1

k2

= [ ] .[m][
. ], [k ] =
T

k1 = 10100 N .m

10000

2000

k1

2000

10400

k2

10100

199000

k 2 = 199000 N .m

Our uncoupled System is,

x

= [ ].

1
2

" #

$ %

&

(
..

m1 .1 + C1 .1 + k11 = F1 (mod al _ force)

..

m2 . 2 + C 2 . 2 + k 2 2 = F2 (mod al _ force)
m1, c1, k1, m2, k2, c2, are already calculated. We should find only F1 and F2 which are
model forces.

Modal Damping: (For SUV Car)

C1'
0

C.(l1 l 2 )

2.C

[C '] =

C.(l1 l 2 ) C.(l12 + l12 )

3000

750

750

4875

2920
0
0
T
= [ '] .[C '][
. '] =
'
0
21280
C2

C1' = 2920 N .s m , C 2' = 212280 N .s m

Modal Stiffness: (For SUV Car)

k .(l1 l 2 )

2.k

[k '] =

2
1

2
1

k .(l1 l 2 ) k .(l + l )

k1'

'
2

29200

212800

30000

7500

7500

48750

k1' = 29200 N .m , k 2' = 2122800 N .m

Our uncoupled system is:

x

= [ ].

1
2

..

..

m1' . + C1' . + k1' = F1' (mod al _ force)

m2' . + C 2' . + k 2' = F2' (mod al _ force)

" #

$ %

&

Calculation of Modal forces:

F1
F2

=[

g1(t)

]T

(6)

g2 (t)

Finding g1 and g2

Calculate g 1 (t ) and g 2 (t )
g 1 (t ) = c1 z1 + c 2 z 2 + k1 z1 + k 2 z 2
z1 = [1 U tA (t )] h o sin

vt
D

z 2 = [U tL (t ) U tP (t )] h o sin
2

v
D

z1 =

vt
D

v
D

[1 U tP (t )] h o cos

vt

z2 =

v
D

[U tL (t ) U tP (t )] h o cos
2

vt
D

= f

g1(t) = c1wf ho[1UtP (t)]coswf t + c2wf ho[UtL(t) UtP (t)]coswf t + kiho[1UtP (t)]sinwf t + k2ho[UtL(t) UtP (t)]sinsinwf t
1

g1(t) = ho[1UtP (t)](k1 sinwf t + c1wf coswf t) + ho[UtL(t) UtP (t)](k2 sinwf t + c2wf coswf t)
1

The same method for g2(t) = c1l1z1 + k1l1z1 c2l2z2 k2l2z2

g2 (t) = hol1[1UtP (t)](k1 sinwf t + c1wf coswf t) hol2[UtL(t) UtP (t)](k2 sinwf t + c2wf coswf t)
1

Byusing VBA Codesin RungeKuttaMethod,we willhave a numericalsolutionfor x,x, ,

D
L
L+ D
Note thatin codes: tP = , tL = , tP =
V
V
V
1

" #

$ %

&

So we should calculateg1(t) andg2(t) and put themin(6) withho = 0.1m D =1.5 (heightand lengthof thebumb)
andL = L1 + L2 = 0.8+1.2 = 2 m, K = 5000,c = 500

1) V = 15 mph = 6.7 m/sec f =

v
D

6.7
1.5

= 14.03 rad/sec

D 1.5
=
= 0.224 sec t P = 0.22 sec
v 6.7
v
2
tL = =
= 0.298 sec t L = 0.3 sec
L 6.7
L+D
tP =
= 0.522 sec t P = 0.52 sec
v
tP =
1

z1, z2 vs. time

0.12

z1, z2 (height, meters)

0.1
0.08
0.06

z1
z2

0.04
0.02
0
0

0.1

0.2

0.3

0.4

0.5

0.6

-0.02
time

" #

$ %

&

Insert the numbers into formula,

g1 (t ) = [1 U 0.22 (t )][500sin(14.03t ) + 701.5 cos(14.03t )] + [U 0.3 (t ) U 0.52 (t )][500sin(14.03t ) + 701.5 cos(14.03t )]
g 2 (t ) = [1 U 0.22 (t )][400sin(14.03t ) + 562.2 cos(14.03t )] + [U 0.3 (t ) U 0.52 (t )][600sin(14.03t ) + 841.8 cos(14.03t )]

2) v = 27 mph = 12 m/sec w f =

v 12
D

1.5

= 25.13 rad/sec

D 1.5
=
= 0.125sec t P = 0.12 sec
v 12
v 2
t L = = = 0.167 sec t L = 0.17 sec
L 12
L + D 3.5
tP =
=
= 0.292 sec t P = 0.29sec
v
12
tP =
1

With this velocity the graphs for g1 (t ) & g 2 (t ) are:

g1, g2 vs. time
1500
1000

g1, g2

500
0
0

0.1

0.2

0.3

0.4

0.5

0.6

g1
g2

-500
-1000
-1500
-2000
time

" #

$ %

&

2) v = 27 mph = 12 m/sec wf =

v
D

12
1.5

= 25.13 rad/sec

D 1.5
=
= 0.125sec t P 1 = 0.12 sec
v 12
v 2
t L = = = 0.167 sec t L = 0.17 sec
L 12
L + D 3.5
=
= 0.292 sec t P2 = 0.29sec
t P2 =
v
12
tP1 =

Insert he
t number into formula,
g1(t) = [1 U0.12(t)][500sin(25.13t) +1256.5cos(25.13t)] + [U0.17(t) U0.29(t)][500sin(25.13t) +1256.5cos(25.13t)]
g2 (t) = [1 U0.12(t)][400sin(25.13t) +1005.2cos(25.13t)] + [U0.17(t) U0.29(t)][600sin(25.13t) +1507.8cos(25.15t)]

3) v = 47 mph = 21m/sec wf =

= 44rad/sec
D
t P 1 = 0.07sec, tL = 0.09sec, tP2 = 0.16sec
g1(t) = [1 U0.07(t)][500sin(44t) + 2200cos(44t)] + [U0.09(t) U0.16(t)][500sin(44t) + 2200cos(44t)]
g2 (t) = [1 U0.07(t)][400sin(44t) +1760cos(44t)] + [U0.09(t) U0.16(t)][600sin(44t) + 2640cos(44t)]
F1

F2

1
1

0.409
- 4.075

F1 = g1 (t ) + 0.409 g 2 (t )

g1 (t )
g 2 (t )
and F2 = g1 (t ) 4.075 g 2 (t )

(8)

For all cases , we have " Uncoupled Equation"

11001 + 10101 + 101001 = g1 (t ) + 0.409 g 2 (t )
109632 + 19900 2 + 199000 2 = g1 (t ) 4.075 g 2 (t )

" #

$ %

&

General solution

k = e

k wk t

[ Ak cos wdk t + ( Bk +

w dk = wk 1 k

Where

wdk

mk

) sin wdk ] +

1
Fk( )e
mk wdk 0

k w w Ak

sin wdk (t ) d

(7)

x0

[ ]T [m]
Ak =

k ww Ak

k [m]
Bk =

x0

wdk mk

By assuming that initial condition is zero, we have A k = B k = 0

t

1
Fk( )e
wdk mk 0

k =

k w w Ak

sin wdk (t ) d

k = 1 .2

w1 = 3.03, m1 = 1100 ,

1 =

w2 = 4.26, m 2 = 10963, 2 =

c1
0.151
2m1w1
c2
0.213
2 m2 w2
2

and wd 1 = w1 1 - 1 = 2.99 rad/sec

2

wd 2 = w2 1 - 2 = 4.16 rad/sec
t

1 =

1
F1 ( z )e 0.46 z sin 2.99(t z )dz
328 0

2 =

1
F2 ( z )e 0.9 z sin 4.16(t z )dz
45606 0

(9)

Substitude from (8) in (9) with different speed which caused if different F1 (t), F2 (t)
we can calculated those integral but even in MATLAB it is so complecated to be evluated, so the numerical
solutions are advised
x

= [ ]

1
1
=
2
0.409

1
- 4.075

1
2

x = 1 + 2

= 0.4091 4.075 2

" #

$ %

&

Differential Equation of Motion Using Lagrange Eq

1
m x + I 0 2
2
1
1
V = k 2 ( x l 2 ) 2 + k 1 ( x + l1 ) 2
2
2
T=

R=

1
1
c 2 ( x l 2 ) 2 + c1 ( x + l1 ) 2
2
2

w = Q1 x + Q 2

1)

T
T R V

+
+
= Q1
t x
x x x

2)

T
T R V

+
+
= Q2
t

x.equation :

T
=0
x

T
T
= mx
= mx
t x
x

R
= c 2 ( x l 2 ) + c1 ( x + l1 )
x

V
= k 2 ( x l 2 ) + k 1 ( x + l1 )
x
w = ( c 1 z 1 + k 1 z 1 + c 2 z 2 + k 2 z 2 ) x + ( c 1 l 1 z 1 c 2 l 2 z 2 + k 1 l 1 z 1 k 2 l 2 z 2 )
Q1 = c1 z 1 + k 1 z 1 + c 2 z 2 + k 2 z 2
mx + c 2 ( x l 2 ) + c1 ( x + l1 ) + k 2 ( x l 2 ) + k 1 ( x + l1 ) = Q1
mx + (c1 + c 2 ) x + (c1 l1 c 2 l 2 ) + ( k 1 + k 2 ) x + ( k 1 l1 k 2 l 2 ) = c1 z 1 + c 2 z 2 + k 1 z 1 + k 2 z 2

" #

$ %

&

mx + c2 ( x l2 ) + c1 ( x + l1 ) + k2 ( x l2 ) + k1 ( x + l1 ) = Q1
mx + (c1 + c2 ) x + (c1l1 c2l2 ) + ( k1 + k2 ) x + (k1l1 k2l2 ) = c1 z1 + c2 z2 + k1 z1 + k2 z2

.equation :

T
T
= I
= I
t

T
=0

R
= - c2l2 ( x l2 ) + c1l1 ( x + l1 )

V
= k2l2 ( x l2 ) + k1l1 ( x + l1 )

and Q1 = c1l1 z1 c2l2 z2 + k1l1 z1 k2l2 z2

2

I + +(c1l1 c2l2 ) x + (c2l2 + c1l1 ) + (k1l1 k2l2 ) x + ck1l1 + k2l2 ) = c1l1 z1 c2l2 z2 + k1l1 z1 k2l2 z2

Put them in matrix format:

m 0
0

..

. x.. +

c1
c1l1 c 2 l 2

c1l1 c 2 l 2 x
k1 + k 2
. . +
2
2
c 2 l 2 + c1l1
k1l1 k 2 l 2

k1l1 k 2 l 2 x
.
k 2 l 22 + k1l12

c1 z 1 + c 2 z 2 + k1 z1 + k 2 z 2
.

c1l1 z 1 c 2 l 2 z 2 + k1l1 z1 k 2 l 2 z 2

" #

$ %

&

Differential Equation of Motion 4DOF (linear)

T=

1
1
1
2
2
2
mx1 + I 0 2 + m3 x 3 + m 2 x 2
2
2
2

V=

1
1
1
1
k 2 ( x1 l 2 x 3 ) 2 + k 1 ( x1 + l1 x 2 ) 2 + k 3 ( x 3 z 2 ) 2 + k 4 ( x 2 z1 ) 2
2
2
2
2

R=

1
1
c 2 ( x1 l 2 x 3 ) 2 + c1 ( x1 + l1 x 2 ) 2
2
2

In this case we bring

z1 , z2 in the potential energy, so we don' t have any Q1 , Q2

1)

T
T R V

+
+
=0
t x1
x1 x1 x1

2)

T
T R V

+
+
=0
t

3)

T
T R V

+
+
=0
t x2
x2 x2 x2

4)

T
T R V

+
+
=0
t x3
x3 x3 x3

" #

$ %

&

'

1) m1 x1 + c2 ( x1 l2 x3 ) + c1 ( x1 + l1 x2 ) + k2 ( x1 l2 x3 ) + k1 ( x1 + l1 x2 ) = 0
m1 x1 + (c1 + c2 ) x1 + (c1l1 c2l2 ) c1 x2 c2 x3 + (k1 + k2 ) x1 + (k1l1 k2l2 ) k1 x2 k2 x3 = 0

2) I + (c2l2 )( x1 l2 x3 ) + c1l1 ( x1 + l1 x2 ) + (k2l2 )( x1 l2 x3 ) + k1l1 ( x1 + l1 x2 ) = 0

2

I + (c2l2 + c1l1 ) + (c1l1 c2l2 ) x1 c1l1 x2 + c2l2 x3 + (k1l1 + k2l2 ) + (k1l1 k2l2 ) x1 k1l1 x2 + k2l2 x3 = 0

3) m2 x2 + (c2 )( x1 + l1 x2 ) + (k1 )( x1 + l1 x2 ) + k4 ( x2 z1 ) = 0
m2 x2 + c1 x2 c1 x1 c1l1 + (k1 + k4 ) x2 k1 x1 k1l1 k4 z1 = 0

4) m3 x3 + (c2 )( x1 l2 x3 ) + (k2 )( x1 l2 x3 ) + k3 ( x3 z2 ) = 0
m3 x3 + c2 x3 c2 x1 + c2l2 + (k2 + k3 ) x3 k2 x1 + k2l2 k3 z2 = 0

Put them in matrix format:

m1

0
0

I
0

0
m2

0
0

m3

x3

k1 + k 2
+

c1 + c 2

x1

x2

k 1 l1 k 2 l 2
2

c1l1 c 2 l 2
2

c1

c2

x1

- c1l1

c2l2

c1l1 c 2 l 2

c1 l 1 + c 2 l 2

c1

- c1l1

c1

x2

-c2

c2l2

c2

x3

k1

k2

x1

- k 1 l1

k 2l2

k 1 l1 k 2 l 2

k 1 l1 + k 2 l 2

- k1

- k 1 l1

k1

x2

-k2

k2l2

k2

x3

0
0
k 4 z1
k3 z2

Note : In this case we have eight state variables.

" #

$ %

&

Differential Equation of Motion 2DOF (Non linear)

1
mx + I 0 2
2
1
1
V = k 2 ( x l 2 sin ) 2 + k1 ( x + l1 sin ) 2
2
2
T=

R=

1
1
c 2 ( x l 2 cos . ) 2 + c1 ( x + l1 cos . ) 2
2
2

1)

T
T R V

+
+
= Q1
t x
x x x

2)

T
T R V

+
+
= Q2
t

1) mx + c2 ( x l2 cos . ) + c1 ( x + l1 cos . ) + k 2 ( x l2 sin ) + k1 ( x + l1 sin ) = c1 z1 + c2 z 2 + k1 z1 + k 2 z 2

mx + (c1 + c2 ) x + (c1l1 cos c2l2 cos ) + (k1 + k 2 ) x + (k1l1 k 2l2 ) sin = c1 z1 + c2 z 2 + k1 z1 + k 2 z2

2) I c2l2 cos ( x l2 cos . ) + c1l1 cos ( x + l1cos ) k 2l2 cos ( x l2 sin ) + k1l1 cos ( x + l1 sin ) = Q2
where Q2 = c1l1 cos z1 c2l2 cos z 2 + k1l1 cos z1 k 2l2 cos z 2

I + (c2l2 cos 2 + c1l1 cos 2 ) + (c1l1 cos c2l2 cos ) x + (k1l1 cos + k 2l2 cos ) sin
+ (k1l1 cos k 2l2 cos ) x = (c1l1 z1 c2l2 z 2 + k1l1 z1 k 2l2 z 2 ) cos

Note: The damping constant is also Non-linear, but we assume that it is linear

" #

$ %

&

Calculating X(t), (t)

( with Laplace Method
x1

m1 0
0

c1 + c2
c1l1 c2l2

c1l1 c2l2
2

c1l1 + c2l2

assume : x(0) = x(0) = 0,

x1

k1 + k 2

k1l1 k 2l2
2
11

k1l1 k2l2

k l + k 2 l2

x
2

g1 (t )
g 2 (t )

(0) = (0) = 0

[ms 2 + (c1 + c 2 ) s + (k1 + k 2 )] + [(c1l1 c 2 l 2 ) s + k1l1 k 2 l 2 ] = L [g 1 (t )]

2

[(c1l1 c 2 l 2 ) s + k1l1 k 2 l 2 ] + [ Is 2 + (c1l1 + c 2 l 2 )s + (k1l1 + k 2 l 2 )] = L [ g 2 (t )]

Det
=
Det

L [g 1 (t )]

(c1l1 c 2 l 2 ) s + (k1l1 k 2 l 2 )

L [ g 2 (t )]

Is 2 + (c1l1 + c 2 l 2 )s + (k1l1 + k 2 l 2 )

ms 2 + (c1 + c 2 ) s + (k1 + k 2 ) (c1l1 c 2 l 2 ) s + (k1l1 k 2 l 2 )

2

(c1l1 c 2 l 2 ) s + (k1l1 k 2 l 2 ) Is 2 + (c1l1 + c 2 l 2 )s + (k1l1 + k 2 l 2 )

ms 2 + (c1 + c 2 ) s + (k1 + k 2 )

L [g1 (t )]

(c1l1 c 2 l 2 ) s + (k1l1 k 2 l 2 )

L [ g 2 (t )]

Det

ms + (c1 + c 2 ) s + (k1 + k 2 ) (c1l1 c 2 l 2 ) s + (k1l1 k 2 l 2 )

2

(c1l1 c 2 l 2 ) s + (k1l1 k 2 l 2 ) Is 2 + (c1l1 + c 2 l 2 )s + (k1l1 + k 2 l 2 )

Note: It is obvious that for solution we must have ay least one of the Nonzero initial condition Or Force (displacement) excitation
By taking an inverse, we will find the solution; however this method usually
is not easy.

" #

$ %

&

Calculating X(t), (t)

( with Numerical method
Runge Kutta (4)
The general format of Runge Kutta method is based on state variables;
however the formulation of this method is provided:
y1
= f1 (t , y1 , y 2 ,... y m )
t
y 2
= f1 (t , y1 , y 2 ,.. y m )
t

y m
= f 2 (t , y1 , y 2 ,... y m )
t
y m,(i +1) = y m,(i ) +

k1, ( m ) + 2k 2, ( m ) + 2k 3,( m ) + k 4,( m )

6
m represent the number of state variables
For example:
k1,1 : k 1 state var iable 1

k1, 2 : k1 state var iable2

k 2 ,1 : k 2 state var iable 1

k 2, 2 : k 2 state var iable2

k 3,1 : k 3 state var iable 1

k 3, 2 : k 3 state var iable2

k 4 ,1 : k 4 state var iable 1

k 4, 2 : k 4 state var iable2

k1,1 = h * f1 (t i , y1( i ) , y 2 ( i ) )
k1,1
k1, 2
h
k 2,1 = h * f1 (t i + , y1( i ) +
, y 2(i ) +
2
2
2
k
h
3,1
k 3,1 = h * f 1 (t i + , y1,( i ) +
, y 2 ( i ) + k 3, 2 )
2
2
k 4,1 = h * f1 (t i + h, y1,( i ) + k 3,1 , y 2 ( i ) + k 3, 2 )

Note: The main point is to finding the State variables

" #

$ %

&

State variables for 2DOF (linear)

mx + (c1 + c2 ) x + (c1l1 c2l2 ) + (k1 + k 2 ) x + (k1l1 k 2l2 ) = c1 z1 + c2 z2 + k1 z1 + k2 z2
2

I + (c1l1 c2l2 ) x + (c2l2 + c1l1 ) + (k1l1 k 2l2 ) x + ck1l1 + k 2l2 ) = c1l1 z1 c2l2 z2 + k1l1 z1 k 2l2 z2
Assume : c1 z1 + c2 z2 + k1 z1 + k2 z2 = g1 (t )
c1l1 z1 c2l2 z2 + k1l1 z1 k2l2 z2 = g 2 (t )
y1 = x

y2 = x

y3 =

y4 =

my 2 + (c1 + c2 ) y 2 + (c1l1 c2l2 ) y 4 + (k1 + k2 ) y1 + (k1l1 k 2l2 ) y 2 = g1 (t )

2

I y 4 + (c1l1 c2l2 ) y 2 + (c2l2 + c1l1 ) y 4 + (k1l1 k2l2 ) y1 + (k1l1 + k 2l2 ) y3 = g 3 (t )

y1 = y 2

, y3 = y 4

In this case we have four state variables

y1
y2
y3
y4

0
(k + k 2 )
- 1
m
=
0
-

( k1l1 k 2 l 2 )
I

1
(c + c 2 )
- 1
m
0
-

(c1l1 c 2 l 2 )
I

0
(c l c 2 l 2 )
- 11
m
1

(k l k 2 l 2 )
- 11
m
0
2

( k1l1 + k 2 l 2 )
I

y2
y3

y4
c1l1 + c 2 l 2
I
Matrix A

Each line gives us one gradient function

" #

$ %

0
g 1 (t )

y1

&

0
g 2 (t )

With previous state variable and using runge kutta method the solution is:
(Note: the related code is available in appendix)
linear silution v=6.7 m/sec runge kutta method
0.15

0.1

0.05
Series1

0
0

0.1

0.2

0.3

0.4

0.5

0.6

-0.05

-0.1

-0.15
time

linear solution with runge kutta v=6.7 m/sec

0.15

0.1

Tetha

0.05

Series1
0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

-0.05

-0.1

-0.15
time

" #

$ %

&

State variables for 2DOF (non linear)

1) mx + (c1 + c 2 ) x + (c1l1 cos c 2 l 2 cos ) + ( k1 + k 2 ) x + (k1l1 k 2 l 2 ) sin = c1 z1 + c 2 z 2 + k1 z1 + k 2 z 2

2) I + (c 2 l 2 cos 2 + c1l1 cos 2 ) + (c1l1 cos c 2 l 2 cos ) x + (k1l1 cos + k 2 l 2 cos ) sin
+ (k1l1 cos k 2 l 2 cos ) x = (c1l1 z1 c 2 l 2 z 2 + k1l1 z1 k 2 l 2 z 2 ) cos

We have four state variables in this case.

State Variables
y1 = x
1) y 1 = y 2
y2 = x

3) y 3 = y 4

y3 =
y4 =
2) my 2 + (c1 + c 2 ) y 2 + (c1 l1 cos y 3 c 2 l 2 cos y 3 ) y 4 + ( k 1 + k 2 ) y 1 + ( k 1 l1 k 2 l 2 ) cos y 3 = g 1 (t )

y2 =

(c1 + c 2 ) y 2 (c1 l1 cos y 3 c 2 l 2 cos y 3 ) y 4 ( k 1 + k 2 ) y 1 ( k 1 l1 k 2 l 2 ) cos y 3 + g 1 (t )

m

where : g 1 (t ) = c1 z 1 + c 2 z 2 + k 1 z 1 + k 2 z 2
2

y4 =

(c1 l1 cos 2 y 3 + c 2 l 2 cos 2 y 3 ) y 4 + (c1 l1 cos y 3 c 2 l 2 cos y 3 ) y 2 + ( k 1 l1 cos y 3 + k 2 l 2 cos y 3 ) sin y 3

I

where : g 3 (t ) = (c1 l1 z 1 c 2 l 2 z 2 + k 1 l1 z 1 k 2 l 2 z 2 ) cos y 3

The graphs are in the next page

" #

$ %

&

'

nonlinear v=6.7 runge kutta

0.15

0.1

0.05
Series1

0
0

0.1

0.2

0.3

0.4

0.5

0.6

-0.05

-0.1

-0.15
time

nonlinear
0.15

0.1

tetha

0.05

0
0

0.1

0.2

0.3

0.4

0.5

0.6

Series1

0.7

-0.05

-0.1

-0.15
time

" #

$ %

&

'

Sample of Excel worksheet:

!
"
#
#$
$
%
&
'

!
!

!"
# $

*
(

"
)

%!

() )" *
** * "*
* ("*)""
"
** )
*))
"
("*"
**(( (
*"*)((
"**

$* *

* +$
**" "
*
(
**(
((
* ( *""
"(" ()
(
"
)(** *
(
)
(

" #

$ %

( * (
*
) ( )
)( "
" )))*)
(
* " )((
(
(
("
) "" "

&

** *+$
( ("*
( ))""
) *" (
) " (
") () "
" ("
*)
( "
(
*( (

'

VBA codes:
Option Base 1
Function u_tp1(t_in, t_p1)
If t_in >= t_p1 Then
u_tp1 = 1
ElseIf t_in < t_p1 Then
u_tp1 = 0
End If
End Function
Function u_tp2(t_in, t_p2)
If t_in >= t_p2 Then
u_tp2 = 1
ElseIf t_in < t_p2 Then
u_tp2 = 0
End If
End Function
Function u_tL(t_in, t_L)
If t_in > t_L Then
u_tL = 1
ElseIf t_in <= t_L Then
u_tL = 0
End If
End Function
Function u_tp(t_in, t_p1)
If t_in > t_p1 Then
u_tp = 1
ElseIf t_in <= t_p1 Then
u_tp = 0
End If
End Function
Function g_1(array_in, xin, t, dt)
Dim c_1 As Double, c_2 As Double ' damping coefficients
Dim k_1 As Double, k_2 As Double ' spring constants for suspension
k_1 = array_in(1, 1)
k_2 = array_in(2, 1)
c_1 = array_in(3, 1)
c_2 = array_in(4, 1)

' spring constants for suspension

' damping coefficients

g_1 = c_1 * z_dot1(array_in, xin, t, dt) + c_2 * z_dot2(array_in, xin, t, dt) + _

k_1 * z_1(array_in, xin, t, dt) + k_2 * z_2(array_in, xin, t, dt)

" #

$ %

&

''

End Function
Function g_2(array_in, xin, t, dt)
Dim c_1 As Double, c_2 As Double ' damping coefficients
Dim k_1 As Double, k_2 As Double ' spring constants for suspension
Dim l_1 As Double, l_2 As Double
k_1 = array_in(1, 1)
k_2 = array_in(2, 1)
c_1 = array_in(3, 1)
c_2 = array_in(4, 1)
l_1 = array_in(11, 1)
l_2 = array_in(12, 1)
g_2 = c_1 * l_1 * z_dot1(array_in, xin, t, dt) - (c_2 * l_2 * z_dot2(array_in, xin, t, dt)) + _
k_1 * l_1 * z_1(array_in, xin, t, dt) - (l_2 * k_2 * z_2(array_in, xin, t, dt))
End Function

Function z_1(array_source, xin, t, dt)

Dim velocity As Double
Dim time As Double
Dim wheelbase As Double, bump_length As Double
Dim h_0 As Double
Dim tp_1 As Double
velocity = array_source(7, 1)
time = t
wheelbase = array_source(9, 1)
h_0 = array_source(10, 1)
tp_1 = tp1(array_source) 'call fucntion to calculate tp1
bump_length = array_source(19, 1)
z_1 = (1 - (u_tp1(time, tp_1))) * h_0 * _
Sin((WorksheetFunction.Pi * velocity * time) / bump_length)
End Function
Function z_2(array_source, xin, t, dt)
Dim velocity As Double
Dim time As Double
Dim wheelbase As Double, bump_length As Double
Dim h_0 As Double
Dim tL As Double, t_p2 As Double
bump_length = array_source(19, 1)
velocity = array_source(7, 1)
time = t
wheelbase = array_source(9, 1)
h_0 = array_source(10, 1)
tL = t_L(array_source)
t_p2 = tp2(array_source)
z_2 = ((u_tL(time, tL)) - (u_tp2(time, t_p2))) * _
h_0 * Sin((WorksheetFunction.Pi * velocity * time) / bump_length)

End Function
Function z_dot1(array_source, xin, t, dt)
Dim t_in As Double, t_p1 As Double 'time constants
Dim velocity As Double
Dim time As Double

" #

$ %

&

'(
Dim wheelbase As Double
Dim h_0 As Double
Dim X As Variant
Dim bump_length As Double
X = xin
bump_length = array_source(19, 1)
t_in = array_source(5, 1)
t_p1 = tp1(array_source)
velocity = array_source(7, 1)
time = t
wheelbase = array_source(9, 1)
h_0 = array_source(10, 1)
z_dot1 = ((WorksheetFunction.Pi * velocity) / bump_length) * (1 - (u_tp1(time, t_p1))) _
* h_0 * Cos((WorksheetFunction.Pi * velocity * time) / bump_length)
End Function
Function z_dot2(array_source, xin, t, dt)
Dim t_in As Double, t_p1 As Double 'time constants
Dim velocity As Double
Dim time As Double
Dim wheelbase As Double
Dim h_0 As Double, bump_length As Double
Dim t_p2 As Double, tL As Double
t_in = array_source(5, 1)
t_p1 = array_source(6, 1)
velocity = array_source(7, 1)
time = t
wheelbase = array_source(9, 1)
h_0 = array_source(10, 1)
bump_length = array_source(19, 1)
t_p2 = tp2(array_source)
tL = t_L(array_source)
z_dot2 = ((WorksheetFunction.Pi * velocity) / bump_length) _
* ((u_tL(time, tL)) - (u_tp2(time, t_p2))) * h_0 * _
Cos((WorksheetFunction.Pi * velocity * time) / bump_length)
End Function
Function f_1prime(array_in, xin, t, dt)
Dim X As Variant
X = xin
f_1prime = X(1, 2)
End Function
Function f_3prime(array_in, xin, t, dt)
Dim X As Variant
X = xin
f_3prime = X(1, 4)
End Function
Function f_2prime(array_in, xin, t, dt)
Dim c_1 As Double, c_2 As Double ' damping coefficients
Dim k_1 As Double, k_2 As Double ' spring constants for suspension
Dim l_1 As Double, l_2 As Double ' lengths

" #

$ %

&

'
Dim y_1 As Double, y_2 As Double
Dim y_3 As Double, y_4 As Double
Dim mass As Double
Dim X As Variant
X = xin
y_2 = X(1, 2)
k_1 = array_in(1, 1)
k_2 = array_in(2, 1)
c_1 = array_in(3, 1)
c_2 = array_in(4, 1)
l_1 = array_in(11, 1)
l_2 = array_in(12, 1)
y_1 = X(1, 1)
y_3 = X(1, 3)
y_4 = X(1, 4)
mass = array_in(17, 1)
f_2prime = -((k_1 + k_2) * y_1) / mass + (-((c_1 + c_2) * y_2) / mass) _
+ (-(k_1 * l_1 - k_2 * l_2) * y_3) / mass + (-(c_1 * l_1 - c_2 * l_2) * y_4 / mass) _
+ g_1(array_in, xin, t, dt) / mass
End Function
Function f_4prime(array_in, xin, t, dt)
Dim c_1 As Double, c_2 As Double ' damping coefficients
Dim k_1 As Double, k_2 As Double ' spring constants for suspension
Dim l_1 As Double, l_2 As Double ' lengths
Dim y_1 As Double, y_2 As Double
Dim y_3 As Double, y_4 As Double
Dim mass As Double, radius As Double
Dim inertia As Double
Dim p1 As Double, p2 As Double
Dim p3 As Double, p4 As Double, p5 As Double
Dim X As Variant
X = xin
y_2 = X(1, 2)
y_4 = X(1, 4)
k_1 = array_in(1, 1)
k_2 = array_in(2, 1)
c_1 = array_in(3, 1)
c_2 = array_in(4, 1)
l_1 = array_in(11, 1)
l_2 = array_in(12, 1)
y_1 = X(1, 1)
y_3 = X(1, 3)
mass = array_in(17, 1)
radius = array_in(18, 1)
inertia = Inertia_moment(mass, radius)
p1 = -(((k_1 * l_1) - (k_2 * l_2)) * y_1) / inertia
p2 = -(((c_1 * l_1) - (c_2 * l_2)) * y_2) / inertia
p3 = -(((k_1 * (l_1) ^ 2) - (k_2 * (l_2) ^ 2)) * y_3) / inertia
p4 = -(((c_1 * (l_1) ^ 2) - (c_2 * (l_2) ^ 2)) * y_4) / inertia
p5 = g_2(array_in, xin, t, dt) / inertia

f_4prime = p1 + p2 + p3 + p4 + p5
End Function

Function tp1(array_in)

" #

$ %

&

'
Dim D As Double, v As Double
D = array_in(19, 1) 'length of bump
v = array_in(7, 1)
tp1 = D / v
End Function
Function t_L(array_in)
Dim L As Double
Dim v As Double
L = array_in(9, 1) 'wheelbase
v = array_in(7, 1) 'velocity
t_L = L / v
End Function
Function tp2(array_in)
Dim L As Double
Dim v As Double
Dim D As Double
D = array_in(19, 1) 'length of bump
L = array_in(9, 1) 'wheelbase
v = array_in(7, 1) 'velocity
tp2 = (L + D) / v
End Function
Function rk_4V2(p, xin, t, dt)
Dim X As Variant, n As Double
X = xin 'row array
n = UBound(X, 2) 'number of state variables
K = K_RK4(p, xin, t, dt) 'array function
For i = 1 To n 'state variables
X(1, i) = X(1, i) + (K(1, i) + 2 * K(2, i) + 2 * K(3, i) + K(4, i)) / 6
Next i
rk_4V2 = X
End Function
Function rk_4V3(p, xin, t, dt)
Dim X As Variant, n As Double
Dim x_2 As Variant
Dim B As Variant
'matrix for forcing functions
Dim A_in As Variant
'matrix of input coefficients
Dim Resultant As Variant
'matrix holder for resultant of x-dot minus B
Dim x_out As Variant
ReDim x_out(1, 4)
X = xin 'row array; holds x-dot (gradient function) output matrix
x_2 = p
n = UBound(X, 2) 'number of state variables
K = K_RK4(p, xin, t, dt) 'array function
For i = 1 To n 'state variables
X(1, i) = X(1, i) + (K(1, i) + 2 * K(2, i) + 2 * K(3, i) + K(4, i)) / 6
Next i

" #

$ %

&

')
Dim k_1 As Double, k_2 As Double
Dim c_1 As Double, c_2 As Double
Dim velocity As Double
Dim wheelbase As Double, height As Double
Dim mass As Double, radius As Double
Dim l_1 As Double, l_2 As Double
Dim bump_length As Double
Dim inertia As Double
k_1 = x_2(1, 1)
k_2 = x_2(2, 1)
c_1 = x_2(3, 1)
c_2 = x_2(4, 1)
l_1 = x_2(11, 1)
l_2 = x_2(12, 1)
mass = x_2(17, 1)
radius = x_2(18, 1)
inertia = Inertia_moment(mass, radius)
'k1 5000 spring constant, front (1, 1)
'k2 5000 spring constant, rear (1, 2)
'c1 500 damping constant, front (1, 3)
'c2 500 damping constant, rear (1, 4)
't_in 0 ??
(1, 5)
't_p1 15 ??
(1, 6)
'v 6.7 velocity (m/sec)
(1, 7)
't 0.5 time (sec)
(1, 8)
'L 2 wheelbase (m)
(1, 9)
'h 0.1 height (of bump)
(1, 10)
'l1 0.8 length (front to cg)
(1, 11)
'l2 1.2 length (rear to cg)
(1, 12)
'y_1 0 x0
(1, 13)
'y_2 0 x-dot (derivative)
(1, 14)
'y_3 0 theta0
(1, 15)
'y_4 0 theta-dot (derivative) (1, 16)
'm 2000 mass
(1, 17)
'r0 0.6 radius
(1, 18)
'D 1 Length of bump (m)
(1, 19)
ReDim B(1 To 1, 1 To 4)
ReDim A_in(1 To 4, 1 To 4)
ReDim Resultant(1 To 4, 1 To 1)
Dim g_one As Double
Dim g_two As Double
g_one = g_1(p, xin, t, dt)
g_two = g_2(p, xin, t, dt)
B(1, 1) = 0
B(1, 2) = g_one / mass
B(1, 3) = 0
B(1, 4) = g_two / inertia
A_in(1, 1) = 0
A_in(1, 2) = 1
A_in(1, 3) = 0
A_in(1, 4) = 1
A_in(2, 1) = -(k_1 + k_2) / mass
A_in(2, 2) = -(c_1 + c_2) / mass
A_in(2, 3) = -(k_1 * l_1 - k_2 * l_2) / mass
A_in(2, 4) = -(c_1 * l_1 - c_2 * l_2) / mass
A_in(3, 1) = 0
A_in(3, 2) = 0
A_in(3, 3) = 0
A_in(3, 4) = 1
A_in(4, 1) = -(k_1 * l_1 - k_2 * l_2) / inertia
A_in(4, 2) = -(c_1 * l_1 - c_2 * l_2) / inertia
A_in(4, 3) = -(k_1 * l_1 ^ 2 + k_2 * l_2 ^ 2) / inertia

" #

$ %

&

'*
A_in(4, 4) = -(c_1 * l_1 ^ 2 + c_2 * l_2 ^ 2) / inertia
Dim step As Integer
'evaluate x-dot minus b:
For step = 1 To 4
Resultant(step, 1) = X(1, step) - B(1, step)
Next step
X = WorksheetFunction.MMult(WorksheetFunction.MInverse(A_in), Resultant)

Dim step2 As Integer

For step2 = 1 To 4
x_out(1, step2) = X(step2, 1)
Next step2
rk_4V3 = x_out
End Function
Function Inertia_moment(mass, radius)
Inertia_moment = mass * radius ^ 2
End Function
Function K_RK4(p, Xi, t, dt)
Dim X, Xt As Variant
Dim ns As Integer, i As Integer
X = Xi
Xt = Xi 'twicked x
ns = UBound(X, 2) 'number of state variable
Dim K(): ReDim K(1 To 4, 1 To ns)
Dim F(): ReDim F(1 To 1, 1 To ns)
Dim FT(): ReDim FT(1 To 1, 1 To ns)
F = Fbar(p, X, t, dt)
For i = 1 To ns 'Calculate all K1 values
K(1, i) = F(1, i) * dt 'basically same as Euler
Next i
'------------------------------------------For i = 1 To ns
Xt(1, i) = X(1, i) + K(1, i) / 2
Next i
FT = Fbar(p, Xt, t + dt / 2, dt)
For i = 1 To ns 'Calculate all K2 values
K(2, i) = FT(1, i) * dt 'basically same as Euler
Next i
'-------------------------For i = 1 To ns
Xt(1, i) = X(1, i) + K(2, i) / 2
Next i
FT = Fbar(p, Xt, t + dt / 2, dt)
For i = 1 To ns 'Calculate all K3 values
K(3, i) = FT(1, i) * dt 'basically same as Euler
Next i
'--------------------------------------------For i = 1 To ns
Xt(1, i) = X(1, i) + K(3, i)
Next i
FT = Fbar(p, Xt, t + dt, dt)
For i = 1 To ns 'Calculate all K4 values
K(4, i) = FT(1, i) * dt 'basically same as Euler
Next i

" #

$ %

&

'+
'------------------------------------K_RK4 = K
End Function
Function Fbar(p, Xi, t, dt)
Dim X As Variant
Dim ns As Integer
X = Xi
ns = UBound(X, 2) 'number of state variables
Dim F(): ReDim F(1 To 1, 1 To ns)
F(1, 1) = f_1prime(p, X, t, dt)
F(1, 2) = f_2prime(p, X, t, dt)
F(1, 3) = f_3prime(p, X, t, dt)
F(1, 4) = f_4prime(p, X, t, dt)
Fbar = F
End Function
Function Euler(p, Xi, t, dt)
Dim ns As Integer, i As Integer
Dim x_2 As Variant
Dim X As Variant 'state vector
Dim F As Variant 'state gradient vector-column topology
Dim x_out As Variant
Dim D_Identity As Variant
ReDim D_Identity(1 To 4, 1 To 4)
ReDim x_out(1, 4)
X = Xi 'Initial State - assume row topology
ns = UBound(X, 2) 'number of state variables
x_2 = p
F = Fbar(p, X, t, dt)
For i = 1 To ns 'state variables
X(1, i) = X(1, i) + F(1, i) * dt 'Eulers Eqn
Next i
'X(1, 1) = f_1prime(p, Xi, t, dt)
'X(1, 2) = f_2prime(p, Xi, t, dt)
'X(1, 3) = f_3prime(p, Xi, t, dt)
'X(1, 4) = f_4prime(p, Xi, t, dt)
Dim k_1 As Double, k_2 As Double
Dim c_1 As Double, c_2 As Double
Dim velocity As Double
Dim wheelbase As Double, height As Double
Dim mass As Double, radius As Double
Dim l_1 As Double, l_2 As Double
Dim bump_length As Double
Dim inertia As Double
k_1 = x_2(1, 1)
k_2 = x_2(2, 1)
c_1 = x_2(3, 1)
c_2 = x_2(4, 1)
l_1 = x_2(11, 1)
l_2 = x_2(12, 1)
mass = x_2(17, 1)
radius = x_2(18, 1)
inertia = Inertia_moment(mass, radius)

" #

$ %

&

(
ReDim B(1 To 1, 1 To 4)
ReDim A_in(1 To 4, 1 To 4)
ReDim Resultant(1 To 4, 1 To 1)
Dim g_one As Double
Dim g_two As Double
g_one = g_1(p, xin, t, dt)
g_two = g_2(p, xin, t, dt)
B(1, 1) = 0
B(1, 2) = g_one / mass
B(1, 3) = 0
B(1, 4) = g_two / inertia
A_in(1, 1) = 0
A_in(1, 2) = 1
A_in(1, 3) = 0
A_in(1, 4) = 1
A_in(2, 1) = -(k_1 + k_2) / mass
A_in(2, 2) = -(c_1 + c_2) / mass
A_in(2, 3) = -(k_1 * l_1 - k_2 * l_2) / mass
A_in(2, 4) = -(c_1 * l_1 - c_2 * l_2) / mass
A_in(3, 1) = 0
A_in(3, 2) = 0
A_in(3, 3) = 0
A_in(3, 4) = 1
A_in(4, 1) = -(k_1 * l_1 - k_2 * l_2) / inertia
A_in(4, 2) = -(c_1 * l_1 - c_2 * l_2) / inertia
A_in(4, 3) = -(k_1 * l_1 ^ 2 + k_2 * l_2 ^ 2) / inertia
A_in(4, 4) = -(c_1 * l_1 ^ 2 + c_2 * l_2 ^ 2) / inertia
D_Identity(1, 1) = 1
D_Identity(1, 2) = 0
D_Identity(1, 3) = 0
D_Identity(1, 4) = 0
D_Identity(2, 1) = 0
D_Identity(2, 2) = 1
D_Identity(2, 3) = 0
D_Identity(2, 4) = 0
D_Identity(3, 1) = 0
D_Identity(3, 2) = 0
D_Identity(3, 3) = 1
D_Identity(3, 4) = 0
D_Identity(4, 1) = 0
D_Identity(4, 2) = 0
D_Identity(4, 3) = 0
D_Identity(4, 4) = 1
Dim step As Integer
'evaluate x-dot minus b:
For step = 1 To 4
Resultant(step, 1) = X(1, step) - B(1, step)
Next step
X = WorksheetFunction.MMult(WorksheetFunction.MInverse(A_in), Resultant)
Dim step2 As Integer
For step2 = 1 To 4
x_out(1, step2) = X(step2, 1)
Next step2
Euler = x_out
End Function

" #

$ %

&