DrStS / EVAA

EVAA: Efficient Vehicle dynAmics simulAtor
GNU General Public License v3.0
4 stars 0 forks source link

Add linear 7DOF model with BE integration operator #1

Closed DrStS closed 4 years ago

DrStS commented 5 years ago
clear;clc;
%%%
k=10
k11=1.1;k12=k;k21=1.2;k22=k;k31=1.3;k32=k;k41=1.4;k42=k;
l1=2;l2=1;l3=0.8;l4=1.25;
%%%
K=[k11+k21+k31+k41 -k11*l1-k41*l1+k21*l2+k31*l2 -k11*l3-k21*l3+k31*l4+k41*l4 -k11 0 -k21 0 -k31 0 -k41 0;
   0 l1*l1*k11+l2*l2*k21+l2*l2*k31+l1*l1*k41 l1*l3*k11-l3*l2*k21+l2*l4*k31-l1*l4*k41 l1*k11 0 -l2*k21 0 -l2*k31 0 l1*k41 0;
   0 0 l3*l3*k11+l3*l3*k21+l4*l4*k31+l4*l4*k41 l3*k11 0 l3*k21 0 -l4*k31 0 -l4*k41 0;
   0 0 0 k11+k12 -k12 0 0 0 0 0 0;
   0 0 0 0 k12 0 0 0 0 0 0;
   0 0 0 0 0 k21+k22 -k22 0 0 0 0;
   0 0 0 0 0 0 k22 0 0 0 0;
   0 0 0 0 0 0 0 k31+k32 -k32 0 0;
   0 0 0 0 0 0 0 0 k32 0 0;
   0 0 0 0 0 0 0 0 0 k41+k42 -k42;
   0 0 0 0 0 0 0 0 0 0 k42;]
K=K+K'-diag(diag(K));
%%%
Kred=K;
Kred(5,:)=[];
Kred(:,5)=[];
Kred(6,:)=[];
Kred(:,6)=[];
Kred(7,:)=[];
Kred(:,7)=[];
Kred(8,:)=[];
Kred(:,8)=[];
eig(Kred);
Kred\[1;0;0;0;0;0;0;]
Ktest=Kred(1:3,1:3)
Ktest\[1;0;0;]