Untitled

mail@pastecode.io avatar
unknown
plain_text
2 months ago
10 kB
4
Indexable
Never
    % numerical evaluation of continuous - time dynamics
    A = [[0, 0, 0, cos(psi)*cos(theta), cos(psi)*sin(phi)*sin(theta) - cos(phi)*sin(psi), sin(phi)*sin(psi) + cos(phi)*cos(psi)*sin(theta), sin(psi)*(w*cos(phi) + v*sin(phi)) + cos(psi)*sin(theta)*(v*cos(phi) - w*sin(phi)),                                 -cos(psi)*(u*sin(theta) - cos(theta)*(w*cos(phi) + v*sin(phi))), - sin(psi)*(sin(theta)*(w*cos(phi) + v*sin(phi)) + u*cos(theta)) - cos(psi)*(v*cos(phi) - w*sin(phi)),  0,  0,  0,  0,                    0,                    0, 1, 0, 0]
        [0, 0, 0, cos(theta)*sin(psi), cos(phi)*cos(psi) + sin(phi)*sin(psi)*sin(theta), cos(phi)*sin(psi)*sin(theta) - cos(psi)*sin(phi), sin(psi)*sin(theta)*(v*cos(phi) - w*sin(phi)) - cos(psi)*(w*cos(phi) + v*sin(phi)),                                 -sin(psi)*(u*sin(theta) - cos(theta)*(w*cos(phi) + v*sin(phi))),   cos(psi)*(sin(theta)*(w*cos(phi) + v*sin(phi)) + u*cos(theta)) - sin(psi)*(v*cos(phi) - w*sin(phi)),  0,  0,  0,  0,                    0,                    0, 0, 1, 0]
        [0, 0, 0,         -sin(theta),                              cos(theta)*sin(phi),                              cos(phi)*cos(theta),                                               cos(theta)*(v*cos(phi) - w*sin(phi)),                                           - sin(theta)*(w*cos(phi) + v*sin(phi)) - u*cos(theta),                                                                                                     0,  0,  0,  0,  0,                    0,                    0, 0, 0, 1]
        [0, 0, 0,                   0,                                         rm - b_r,                                         b_q - qm,                                                                                  0,                                                                           -(981*cos(theta))/100,                                                                                                     0, -1,  0,  0,  0,                    w,                   -v, 0, 0, 0]
        [0, 0, 0,            b_r - rm,                                                0,                                         pm - b_p,                                                      (981*cos(phi)*cos(theta))/100,                                                                  -(981*sin(phi)*sin(theta))/100,                                                                                                     0,  0, -1,  0, -w,                    0,                    u, 0, 0, 0]
        [0, 0, 0,            qm - b_q,                                         b_p - pm,                                                0,                                                     -(981*cos(theta)*sin(phi))/100,                                                                  -(981*cos(phi)*sin(theta))/100,                                                                                                     0,  0,  0, -1,  v,                   -u,                    0, 0, 0, 0]
        [0, 0, 0,                   0,                                                0,                                                0,                    sin(phi)*tan(theta)*(b_r - rm) - cos(phi)*tan(theta)*(b_q - qm),               - cos(phi)*(b_r - rm)*(tan(theta)^2 + 1) - sin(phi)*(b_q - qm)*(tan(theta)^2 + 1),                                                                                                     0,  0,  0,  0, -1, -sin(phi)*tan(theta), -cos(phi)*tan(theta), 0, 0, 0]
        [0, 0, 0,                   0,                                                0,                                                0,                                          cos(phi)*(b_r - rm) + sin(phi)*(b_q - qm),                                                                                               0,                                                                                                     0,  0,  0,  0,  0,            -cos(phi),             sin(phi), 0, 0, 0]
        [0, 0, 0,                   0,                                                0,                                                0,                (sin(phi)*(b_r - rm))/cos(theta) - (cos(phi)*(b_q - qm))/cos(theta), - (cos(phi)*sin(theta)*(b_r - rm))/cos(theta)^2 - (sin(phi)*sin(theta)*(b_q - qm))/cos(theta)^2,                                                                                                     0,  0,  0,  0,  0, -sin(phi)/cos(theta), -cos(phi)/cos(theta), 0, 0, 0]
        [0, 0, 0,                   0,                                                0,                                                0,                                                                                  0,                                                                                               0,                                                                                                     0,  0,  0,  0,  0,                    0,                    0, 0, 0, 0]
        [0, 0, 0,                   0,                                                0,                                                0,                                                                                  0,                                                                                               0,                                                                                                     0,  0,  0,  0,  0,                    0,                    0, 0, 0, 0]
        [0, 0, 0,                   0,                                                0,                                                0,                                                                                  0,                                                                                               0,                                                                                                     0,  0,  0,  0,  0,                    0,                    0, 0, 0, 0]
        [0, 0, 0,                   0,                                                0,                                                0,                                                                                  0,                                                                                               0,                                                                                                     0,  0,  0,  0,  0,                    0,                    0, 0, 0, 0]
        [0, 0, 0,                   0,                                                0,                                                0,                                                                                  0,                                                                                               0,                                                                                                     0,  0,  0,  0,  0,                    0,                    0, 0, 0, 0]
        [0, 0, 0,                   0,                                                0,                                                0,                                                                                  0,                                                                                               0,                                                                                                     0,  0,  0,  0,  0,                    0,                    0, 0, 0, 0]
        [0, 0, 0,                   0,                                                0,                                                0,                                                                                  0,                                                                                               0,                                                                                                     0,  0,  0,  0,  0,                    0,                    0, 0, 0, 0]
        [0, 0, 0,                   0,                                                0,                                                0,                                                                                  0,                                                                                               0,                                                                                                     0,  0,  0,  0,  0,                    0,                    0, 0, 0, 0]
        [0, 0, 0,                   0,                                                0,                                                0,                                                                                  0,                                                                                               0,                                                                                                     0,  0,  0,  0,  0,                    0,                    0, 0, 0, 0]];
    
    G = [[ 0,  0,  0,  0,                    0,                    0]
        [ 0,  0,  0,  0,                    0,                    0]
        [ 0,  0,  0,  0,                    0,                    0]
        [-1,  0,  0,  0,                    w,                   -v]
        [ 0, -1,  0, -w,                    0,                    u]
        [ 0,  0, -1,  v,                   -u,                    0]
        [ 0,  0,  0, -1, -sin(phi)*tan(theta), -cos(phi)*tan(theta)]
        [ 0,  0,  0,  0,            -cos(phi),             sin(phi)]
        [ 0,  0,  0,  0, -sin(phi)/cos(theta), -cos(phi)/cos(theta)]
        [ 0,  0,  0,  0,                    0,                    0]
        [ 0,  0,  0,  0,                    0,                    0]
        [ 0,  0,  0,  0,                    0,                    0]
        [ 0,  0,  0,  0,                    0,                    0]
        [ 0,  0,  0,  0,                    0,                    0]
        [ 0,  0,  0,  0,                    0,                    0]
        [ 0,  0,  0,  0,                    0,                    0]
        [ 0,  0,  0,  0,                    0,                    0]
        [ 0,  0,  0,  0,                    0,                    0]];
Leave a Comment