Untitled

    Cxya_old = [P(kk-1,1:3);P(kk-1,4:6);P(kk-1,7:9)];
    Cu =   [0.01*abs(v) 0 0;0 0.01*a 0;0 0 0.01*T];
    Au = [T*cos(a)*cos(A(kk-1)+((T*v*sin(a))/(2*L)))-((T^2)*v*cos(a)*sin(a)*sin(A(kk-1)+((T*v*sin(a))/(2*L))))/(2*L) (((T^2)*v*cos(a)*cos(A(kk-1)+((T*v*sin(a))/(2*L)))*sin(a))/(2*L))+T*cos(a)*sin(A(kk-1)+((T*v*sin(a))/(2*L))) T*sin(a)/L;(-T)*v*cos(A(kk-1)+((T*v*sin(a))/(2*L)))*sin(a)-((T^2)*(v^2)*(cos(a)^2)*sin(A(kk-1)+((T*v*sin(a))/(2*L))))/(2*L) ((T^2)*(v^2)*(cos(a)^2)*cos(A(kk-1)+((T*v*sin(a))/(2*L))))/(2*L)-T*v*sin(a)*sin(A(kk-1)+((T*v*sin(a))/(2*L))) (T*v*cos(a))/L;(v*cos(a)*cos(A(kk-1)+((T*v*sin(a))/(2*L))))-(T*(v^2)*cos(a)*sin(a)*sin(A(kk-1)+((T*v*sin(a))/(2*L))))/(2*L) ((T*(v^2)*cos(a)*cos(A(kk-1)+((T*v*sin(a))/(2*L))))/(2*L))+v*cos(a)*sin(A(kk-1)+((T*v*sin(a))/(2*L))) (v*sin(a))/L];
    Axya =   [1 0 0;0 1 0;(-T*v*cos(a)*sin(A(kk-1)+((T*v*sin(a))/(2*L))))/(2*L) (T*v*cos(a)*cos(A(kk-1)+((T*v*sin(a))/(2*L))))/(2*L) 1];
    Cxya_new = Axya'*Cxya_old*Axya + Au'*Cu*Au;
    
    P(kk,1:9) = [Cxya_new(1,1:3) Cxya_new(2,1:3) Cxya_new(3,1:3)];