-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathtest
More file actions
executable file
·29 lines (25 loc) · 765 Bytes
/
Copy pathtest
File metadata and controls
executable file
·29 lines (25 loc) · 765 Bytes
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
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Tue Sep 24 17:29:48 2019
@author: emil
"""
@jitclass(
def numbafy_func():
x0 = tan(pitch)
x1 = sin(roll)
x2 = q*x1
x3 = cos(roll)
x4 = r*x3
x5 = sin(pitch)
x6 = cos(pitch)
x7 = 1/x6
x8 = g*x6
x9 = cos(yaw)
x10 = u*x6
x11 = sin(yaw)
x12 = x1*x11
x13 = x3*x9
x14 = x11*x3
x15 = x1*x9
return Matrix([[p + x0*x2 + x0*x4], [q*x3 - r*x5], [x2*x7 + x4*x7], [(q*r*(iy - iz) + tx)/ix], [(p*r*(-ix + iz) + ty)/iy], [(p*q*(ix - iy) + tz)/iz], [-g*x5 - q*w + r*v], [p*w - r*u + x1*x8], [-ft/m - p*v + q*u + x3*x8], [-v*(x14 - x15*x5) + w*(x12 + x13*x5) + x10*x9], [v*(x12*x5 + x13) - w*(-x14*x5 + x15) + x10*x11], [-u*x5 + v*x1*x6 + w*x3*x6]])