PX4 Firmware: src/lib/ecl/EKF/matlab/scripts/Inertial Nav EKF/K_LOSX.c File Reference

PX4 Firmware

PX4 Autopilot Software http://px4.io

Go to the source code of this file.

Variables
	t2 = 1.0/range

	t3 = q1vd2.0

	t4 = q0ve2.0

	t11 = q3vn2.0

	t5 = t3+t4-t11

	t6 = q0q32.0

	t29 = q1q22.0

	t7 = t6-t29

	t8 = q0q12.0

	t9 = q2q32.0

	t10 = t8+t9

	t12 = P[0][0]t2t5

	t13 = q0vd2.0

	t14 = q2vn2.0

	t28 = q1ve2.0

	t15 = t13+t14-t28

	t16 = q3vd2.0

	t17 = q2ve2.0

	t18 = q1vn2.0

	t19 = t16+t17+t18

	t20 = q3ve2.0

	t21 = q0vn2.0

	t30 = q2vd2.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]t2t15

	t32 = P[6][0]t2t10

	t33 = P[1][0]t2t15

	t34 = P[2][0]t2t19

	t35 = P[5][0]t2t27

	t79 = P[4][0]t2t7

	t80 = P[3][0]t2t22

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

	t37 = t2t5t36

	t38 = P[6][1]t2t10

	t39 = P[0][1]t2t5

	t40 = P[2][1]t2t19

	t41 = P[5][1]t2t27

	t81 = P[4][1]t2t7

	t82 = P[3][1]t2t22

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

	t43 = t2t15t42

	t44 = P[6][2]t2t10

	t45 = P[0][2]t2t5

	t46 = P[1][2]t2t15

	t47 = P[2][2]t2t19

	t48 = P[5][2]t2t27

	t83 = P[4][2]t2t7

	t84 = P[3][2]t2t22

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

	t50 = t2t19t49

	t51 = P[6][3]t2t10

	t52 = P[0][3]t2t5

	t53 = P[1][3]t2t15

	t54 = P[2][3]t2t19

	t55 = P[5][3]t2t27

	t85 = P[4][3]t2t7

	t86 = P[3][3]t2t22

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

	t57 = P[6][5]t2t10

	t58 = P[0][5]t2t5

	t59 = P[1][5]t2t15

	t60 = P[2][5]t2t19

	t61 = P[5][5]t2t27

	t88 = P[4][5]t2t7

	t89 = P[3][5]t2t22

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

	t63 = t2t27t62

	t64 = P[6][4]t2t10

	t65 = P[0][4]t2t5

	t66 = P[1][4]t2t15

	t67 = P[2][4]t2t19

	t68 = P[5][4]t2t27

	t90 = P[4][4]t2t7

	t91 = P[3][4]t2t22

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

	t70 = P[6][6]t2t10

	t71 = P[0][6]t2t5

	t72 = P[1][6]t2t15

	t73 = P[2][6]t2t19

	t74 = P[5][6]t2t27

	t93 = P[4][6]t2t7

	t94 = P[3][6]t2t22

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

	t76 = t2t10t75

	t87 = t2t22t56

	t92 = t2t7t69

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

	t78 = 1.0/t77

	A0 [0][0] = t78(t12-P[0][4]t2t7+P[0][1]t2t15+P[0][6]t2t10+P[0][2]t2t19-P[0][3]t2t22+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().