PX4 Firmware
PX4 Autopilot Software http://px4.io
K_LOSX.c File Reference

Go to the source code of this file.

Variables

 t2 = 1.0/range
 
 t3 = q1*vd*2.0
 
 t4 = q0*ve*2.0
 
 t11 = q3*vn*2.0
 
 t5 = t3+t4-t11
 
 t6 = q0*q3*2.0
 
 t29 = q1*q2*2.0
 
 t7 = t6-t29
 
 t8 = q0*q1*2.0
 
 t9 = q2*q3*2.0
 
 t10 = t8+t9
 
 t12 = P[0][0]*t2*t5
 
 t13 = q0*vd*2.0
 
 t14 = q2*vn*2.0
 
 t28 = q1*ve*2.0
 
 t15 = t13+t14-t28
 
 t16 = q3*vd*2.0
 
 t17 = q2*ve*2.0
 
 t18 = q1*vn*2.0
 
 t19 = t16+t17+t18
 
 t20 = q3*ve*2.0
 
 t21 = q0*vn*2.0
 
 t30 = q2*vd*2.0
 
 t22 = t20+t21-t30
 
 t23 = q0*q0
 
 t24 = q1*q1
 
 t25 = q2*q2
 
 t26 = q3*q3
 
 t27 = t23-t24+t25-t26
 
 t31 = P[1][1]*t2*t15
 
 t32 = P[6][0]*t2*t10
 
 t33 = P[1][0]*t2*t15
 
 t34 = P[2][0]*t2*t19
 
 t35 = P[5][0]*t2*t27
 
 t79 = P[4][0]*t2*t7
 
 t80 = P[3][0]*t2*t22
 
 t36 = t12+t32+t33+t34+t35-t79-t80
 
 t37 = t2*t5*t36
 
 t38 = P[6][1]*t2*t10
 
 t39 = P[0][1]*t2*t5
 
 t40 = P[2][1]*t2*t19
 
 t41 = P[5][1]*t2*t27
 
 t81 = P[4][1]*t2*t7
 
 t82 = P[3][1]*t2*t22
 
 t42 = t31+t38+t39+t40+t41-t81-t82
 
 t43 = t2*t15*t42
 
 t44 = P[6][2]*t2*t10
 
 t45 = P[0][2]*t2*t5
 
 t46 = P[1][2]*t2*t15
 
 t47 = P[2][2]*t2*t19
 
 t48 = P[5][2]*t2*t27
 
 t83 = P[4][2]*t2*t7
 
 t84 = P[3][2]*t2*t22
 
 t49 = t44+t45+t46+t47+t48-t83-t84
 
 t50 = t2*t19*t49
 
 t51 = P[6][3]*t2*t10
 
 t52 = P[0][3]*t2*t5
 
 t53 = P[1][3]*t2*t15
 
 t54 = P[2][3]*t2*t19
 
 t55 = P[5][3]*t2*t27
 
 t85 = P[4][3]*t2*t7
 
 t86 = P[3][3]*t2*t22
 
 t56 = t51+t52+t53+t54+t55-t85-t86
 
 t57 = P[6][5]*t2*t10
 
 t58 = P[0][5]*t2*t5
 
 t59 = P[1][5]*t2*t15
 
 t60 = P[2][5]*t2*t19
 
 t61 = P[5][5]*t2*t27
 
 t88 = P[4][5]*t2*t7
 
 t89 = P[3][5]*t2*t22
 
 t62 = t57+t58+t59+t60+t61-t88-t89
 
 t63 = t2*t27*t62
 
 t64 = P[6][4]*t2*t10
 
 t65 = P[0][4]*t2*t5
 
 t66 = P[1][4]*t2*t15
 
 t67 = P[2][4]*t2*t19
 
 t68 = P[5][4]*t2*t27
 
 t90 = P[4][4]*t2*t7
 
 t91 = P[3][4]*t2*t22
 
 t69 = t64+t65+t66+t67+t68-t90-t91
 
 t70 = P[6][6]*t2*t10
 
 t71 = P[0][6]*t2*t5
 
 t72 = P[1][6]*t2*t15
 
 t73 = P[2][6]*t2*t19
 
 t74 = P[5][6]*t2*t27
 
 t93 = P[4][6]*t2*t7
 
 t94 = P[3][6]*t2*t22
 
 t75 = t70+t71+t72+t73+t74-t93-t94
 
 t76 = t2*t10*t75
 
 t87 = t2*t22*t56
 
 t92 = t2*t7*t69
 
 t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92
 
 t78 = 1.0/t77
 
 A0 [0][0] = t78*(t12-P[0][4]*t2*t7+P[0][1]*t2*t15+P[0][6]*t2*t10+P[0][2]*t2*t19-P[0][3]*t2*t22+P[0][5]*t2*t27)
 

Variable Documentation

◆ A0

A0[23][0] = t78*(t12-P[0][4]*t2*t7+P[0][1]*t2*t15+P[0][6]*t2*t10+P[0][2]*t2*t19-P[0][3]*t2*t22+P[0][5]*t2*t27)

Definition at line 94 of file K_LOSX.c.

◆ t10

t10 = t8+t9

Definition at line 11 of file K_LOSX.c.

◆ t11

t11 = q3*vn*2.0

Definition at line 4 of file K_LOSX.c.

◆ t12

t12 = P[0][0]*t2*t5

Definition at line 12 of file K_LOSX.c.

◆ t13

t13 = q0*vd*2.0

Definition at line 13 of file K_LOSX.c.

◆ t14

t14 = q2*vn*2.0

Definition at line 14 of file K_LOSX.c.

◆ t15

t15 = t13+t14-t28

Definition at line 16 of file K_LOSX.c.

◆ t16

t16 = q3*vd*2.0

Definition at line 17 of file K_LOSX.c.

◆ t17

t17 = q2*ve*2.0

Definition at line 18 of file K_LOSX.c.

◆ t18

t18 = q1*vn*2.0

Definition at line 19 of file K_LOSX.c.

◆ t19

t19 = t16+t17+t18

Definition at line 20 of file K_LOSX.c.

◆ t2

t2 = 1.0/range

Definition at line 1 of file K_LOSX.c.

◆ t20

t20 = q3*ve*2.0

Definition at line 21 of file K_LOSX.c.

◆ t21

t21 = q0*vn*2.0

Definition at line 22 of file K_LOSX.c.

◆ t22

t22 = t20+t21-t30

Definition at line 24 of file K_LOSX.c.

◆ t23

t23 = q0*q0

Definition at line 25 of file K_LOSX.c.

◆ t24

t24 = q1*q1

Definition at line 26 of file K_LOSX.c.

◆ t25

t25 = q2*q2

Definition at line 27 of file K_LOSX.c.

◆ t26

t26 = q3*q3

Definition at line 28 of file K_LOSX.c.

◆ t27

t27 = t23-t24+t25-t26

Definition at line 29 of file K_LOSX.c.

◆ t28

t28 = q1*ve*2.0

Definition at line 15 of file K_LOSX.c.

◆ t29

t29 = q1*q2*2.0

Definition at line 7 of file K_LOSX.c.

◆ t3

t3 = q1*vd*2.0

Definition at line 2 of file K_LOSX.c.

◆ t30

t30 = q2*vd*2.0

Definition at line 23 of file K_LOSX.c.

◆ t31

t31 = P[1][1]*t2*t15

Definition at line 30 of file K_LOSX.c.

◆ t32

t32 = P[6][0]*t2*t10

Definition at line 31 of file K_LOSX.c.

◆ t33

t33 = P[1][0]*t2*t15

Definition at line 32 of file K_LOSX.c.

◆ t34

t34 = P[2][0]*t2*t19

Definition at line 33 of file K_LOSX.c.

◆ t35

t35 = P[5][0]*t2*t27

Definition at line 34 of file K_LOSX.c.

◆ t36

t36 = t12+t32+t33+t34+t35-t79-t80

Definition at line 37 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow(), and Ekf::initialiseQuatCovariances().

◆ t37

t37 = t2*t5*t36

Definition at line 38 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow(), and Ekf::initialiseQuatCovariances().

◆ t38

t38 = P[6][1]*t2*t10

Definition at line 39 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow(), and Ekf::initialiseQuatCovariances().

◆ t39

t39 = P[0][1]*t2*t5

Definition at line 40 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow(), and Ekf::initialiseQuatCovariances().

◆ t4

t4 = q0*ve*2.0

Definition at line 3 of file K_LOSX.c.

◆ t40

t40 = P[2][1]*t2*t19

Definition at line 41 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow(), and Ekf::initialiseQuatCovariances().

◆ t41

t41 = P[5][1]*t2*t27

Definition at line 42 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow(), and Ekf::initialiseQuatCovariances().

◆ t42

t42 = t31+t38+t39+t40+t41-t81-t82

Definition at line 45 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow(), and Ekf::initialiseQuatCovariances().

◆ t43

t43 = t2*t15*t42

Definition at line 46 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow(), and Ekf::initialiseQuatCovariances().

◆ t44

t44 = P[6][2]*t2*t10

Definition at line 47 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow(), and Ekf::initialiseQuatCovariances().

◆ t45

t45 = P[0][2]*t2*t5

Definition at line 48 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t46

t46 = P[1][2]*t2*t15

Definition at line 49 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t47

t47 = P[2][2]*t2*t19

Definition at line 50 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t48

t48 = P[5][2]*t2*t27

Definition at line 51 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t49

t49 = t44+t45+t46+t47+t48-t83-t84

Definition at line 54 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t5

t5 = t3+t4-t11

Definition at line 5 of file K_LOSX.c.

◆ t50

t50 = t2*t19*t49

Definition at line 55 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t51

t51 = P[6][3]*t2*t10

Definition at line 56 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t52

t52 = P[0][3]*t2*t5

Definition at line 57 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t53

t53 = P[1][3]*t2*t15

Definition at line 58 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t54

t54 = P[2][3]*t2*t19

Definition at line 59 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t55

t55 = P[5][3]*t2*t27

Definition at line 60 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t56

t56 = t51+t52+t53+t54+t55-t85-t86

Definition at line 63 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t57

t57 = P[6][5]*t2*t10

Definition at line 64 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t58

t58 = P[0][5]*t2*t5

Definition at line 65 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t59

t59 = P[1][5]*t2*t15

Definition at line 66 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t6

t6 = q0*q3*2.0

Definition at line 6 of file K_LOSX.c.

◆ t60

t60 = P[2][5]*t2*t19

Definition at line 67 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t61

t61 = P[5][5]*t2*t27

Definition at line 68 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t62

t62 = t57+t58+t59+t60+t61-t88-t89

Definition at line 71 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t63

t63 = t2*t27*t62

Definition at line 72 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t64

t64 = P[6][4]*t2*t10

Definition at line 73 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t65

t65 = P[0][4]*t2*t5

Definition at line 74 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t66

t66 = P[1][4]*t2*t15

Definition at line 75 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t67

t67 = P[2][4]*t2*t19

Definition at line 76 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t68

t68 = P[5][4]*t2*t27

Definition at line 77 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t69

t69 = t64+t65+t66+t67+t68-t90-t91

Definition at line 80 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t7

t7 = t6-t29

Definition at line 8 of file K_LOSX.c.

◆ t70

t70 = P[6][6]*t2*t10

Definition at line 81 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t71

t71 = P[0][6]*t2*t5

Definition at line 82 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t72

t72 = P[1][6]*t2*t15

Definition at line 83 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t73

t73 = P[2][6]*t2*t19

Definition at line 84 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t74

t74 = P[5][6]*t2*t27

Definition at line 85 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t75

t75 = t70+t71+t72+t73+t74-t93-t94

Definition at line 88 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t76

t76 = t2*t10*t75

Definition at line 89 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t77

t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92

Definition at line 92 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t78

t78 = 1.0/t77

Definition at line 93 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t79

t79 = P[4][0]*t2*t7

Definition at line 35 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t8

t8 = q0*q1*2.0

Definition at line 9 of file K_LOSX.c.

◆ t80

t80 = P[3][0]*t2*t22

Definition at line 36 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t81

t81 = P[4][1]*t2*t7

Definition at line 43 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t82

t82 = P[3][1]*t2*t22

Definition at line 44 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t83

t83 = P[4][2]*t2*t7

Definition at line 52 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t84

t84 = P[3][2]*t2*t22

Definition at line 53 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t85

t85 = P[4][3]*t2*t7

Definition at line 61 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t86

t86 = P[3][3]*t2*t22

Definition at line 62 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t87

t87 = t2*t22*t56

Definition at line 90 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t88

t88 = P[4][5]*t2*t7

Definition at line 69 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t89

t89 = P[3][5]*t2*t22

Definition at line 70 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t9

t9 = q2*q3*2.0

Definition at line 10 of file K_LOSX.c.

◆ t90

t90 = P[4][4]*t2*t7

Definition at line 78 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t91

t91 = P[3][4]*t2*t22

Definition at line 79 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t92

t92 = t2*t7*t69

Definition at line 91 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t93

t93 = P[4][6]*t2*t7

Definition at line 86 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ t94

t94 = P[3][6]*t2*t22

Definition at line 87 of file K_LOSX.c.

Referenced by Ekf::fuseOptFlow().