-
Notifications
You must be signed in to change notification settings - Fork 2
/
getERtdt.m
63 lines (57 loc) · 3.9 KB
/
getERtdt.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
function [ ERtdt ] = getERtdt( omega, Miu, P, U, V, H, EQ, dt, omegaLocal, defQS )
ERt = U*EQ*V';
if omegaLocal
if defQS
EQVHatPvRt(1,1) = (V(3,2)*P(2,3)-V(2,2)*P(3,3))*EQ(2,2)+(V(2,3)*P(3,2)-V(3,3)*P(2,2))*EQ(3,3);
EQVHatPvRt(1,2) = (V(1,2)*P(3,3)-V(3,2)*P(1,3))*EQ(2,2)+(V(3,3)*P(1,2)-V(1,3)*P(3,2))*EQ(3,3);
EQVHatPvRt(1,3) = (V(2,2)*P(1,3)-V(1,2)*P(2,3))*EQ(2,2)+(V(1,3)*P(2,2)-V(2,3)*P(1,2))*EQ(3,3);
EQVHatPvRt(2,1) = (V(2,1)*P(3,3)-V(3,1)*P(2,3))*EQ(1,1)+(V(3,3)*P(2,1)-V(2,3)*P(3,1))*EQ(3,3);
EQVHatPvRt(2,2) = (V(3,1)*P(1,3)-V(1,1)*P(3,3))*EQ(1,1)+(V(1,3)*P(3,1)-V(3,3)*P(1,1))*EQ(3,3);
EQVHatPvRt(2,3) = (V(1,1)*P(2,3)-V(2,1)*P(1,3))*EQ(1,1)+(V(2,3)*P(1,1)-V(1,3)*P(2,1))*EQ(3,3);
EQVHatPvRt(3,1) = (V(3,1)*P(2,2)-V(2,1)*P(3,2))*EQ(1,1)+(V(2,2)*P(3,1)-V(3,2)*P(2,1))*EQ(2,2);
EQVHatPvRt(3,2) = (V(1,1)*P(3,2)-V(3,1)*P(1,2))*EQ(1,1)+(V(3,2)*P(1,1)-V(1,2)*P(3,1))*EQ(2,2);
EQVHatPvRt(3,3) = (V(2,1)*P(1,2)-V(1,1)*P(2,2))*EQ(1,1)+(V(1,2)*P(2,1)-V(2,2)*P(1,1))*EQ(2,2);
else
EQVHatPvRt(1,1) = (P(2,3)*V(3,2)-P(3,3)*V(2,2)-P(2,2)*V(3,3)+P(3,2)*V(2,3))*EQ(1,1);
EQVHatPvRt(1,2) = (P(1,2)*V(3,3)-P(3,2)*V(1,3)-P(1,3)*V(3,2)+P(3,3)*V(1,2))*EQ(1,1);
EQVHatPvRt(1,3) = (P(1,3)*V(2,2)-P(2,3)*V(1,2)-P(1,2)*V(2,3)+P(2,2)*V(1,3))*EQ(1,1);
EQVHatPvRt(2,1) = (P(2,1)*V(3,3)-P(3,1)*V(2,3)-P(2,3)*V(3,1)+P(3,3)*V(2,1))*EQ(2,2);
EQVHatPvRt(2,2) = (P(1,3)*V(3,1)-P(3,3)*V(1,1)-P(1,1)*V(3,3)+P(3,1)*V(1,3))*EQ(2,2);
EQVHatPvRt(2,3) = (P(1,1)*V(2,3)-P(2,1)*V(1,3)-P(1,3)*V(2,1)+P(2,3)*V(1,1))*EQ(2,2);
EQVHatPvRt(3,1) = (P(2,2)*V(3,1)-P(3,2)*V(2,1)-P(2,1)*V(3,2)+P(3,1)*V(2,2))*EQ(3,3);
EQVHatPvRt(3,2) = (P(1,1)*V(3,2)-P(3,1)*V(1,2)-P(1,2)*V(3,1)+P(3,2)*V(1,1))*EQ(3,3);
EQVHatPvRt(3,3) = (P(1,2)*V(2,1)-P(2,2)*V(1,1)-P(1,1)*V(2,2)+P(2,1)*V(1,2))*EQ(3,3);
end
ERHatPvRt = U*EQVHatPvRt;
else
if defQS
EHatPvRUQt(1,1) = (P(2,3)*U(3,2)-P(3,3)*U(2,2)-P(2,2)*U(3,3)+P(3,2)*U(2,3))*EQ(1,1);
EHatPvRUQt(1,2) = (P(2,1)*U(3,3)-P(3,1)*U(2,3)-P(2,3)*U(3,1)+P(3,3)*U(2,1))*EQ(2,2);
EHatPvRUQt(1,3) = (P(2,2)*U(3,1)-P(3,2)*U(2,1)-P(2,1)*U(3,2)+P(3,1)*U(2,2))*EQ(3,3);
EHatPvRUQt(2,1) = (P(1,2)*U(3,3)-P(3,2)*U(1,3)-P(1,3)*U(3,2)+P(3,3)*U(1,2))*EQ(1,1);
EHatPvRUQt(2,2) = (P(1,3)*U(3,1)-P(3,3)*U(1,1)-P(1,1)*U(3,3)+P(3,1)*U(1,3))*EQ(2,2);
EHatPvRUQt(2,3) = (P(1,1)*U(3,2)-P(3,1)*U(1,2)-P(1,2)*U(3,1)+P(3,2)*U(1,1))*EQ(3,3);
EHatPvRUQt(3,1) = (P(1,3)*U(2,2)-P(2,3)*U(1,2)-P(1,2)*U(2,3)+P(2,2)*U(1,3))*EQ(1,1);
EHatPvRUQt(3,2) = (P(1,1)*U(2,3)-P(2,1)*U(1,3)-P(1,3)*U(2,1)+P(2,3)*U(1,1))*EQ(2,2);
EHatPvRUQt(3,3) = (P(1,2)*U(2,1)-P(2,2)*U(1,1)-P(1,1)*U(2,2)+P(2,1)*U(1,2))*EQ(3,3);
else
EHatPvRUQt(1,1) = (P(2,3)*U(3,2)-P(3,3)*U(2,2))*EQ(2,2)+(P(3,2)*U(2,3)-P(2,2)*U(3,3))*EQ(3,3);
EHatPvRUQt(1,2) = (P(3,3)*U(2,1)-P(2,3)*U(3,1))*EQ(1,1)+(P(2,1)*U(3,3)-P(3,1)*U(2,3))*EQ(3,3);
EHatPvRUQt(1,3) = (P(2,2)*U(3,1)-P(3,2)*U(2,1))*EQ(1,1)+(P(3,1)*U(2,2)-P(2,1)*U(3,2))*EQ(2,2);
EHatPvRUQt(2,1) = (P(3,3)*U(1,2)-P(1,3)*U(3,2))*EQ(2,2)+(P(1,2)*U(3,3)-P(3,2)*U(1,3))*EQ(3,3);
EHatPvRUQt(2,2) = (P(1,3)*U(3,1)-P(3,3)*U(1,1))*EQ(1,1)+(P(3,1)*U(1,3)-P(1,1)*U(3,3))*EQ(3,3);
EHatPvRUQt(2,3) = (P(3,2)*U(1,1)-P(1,2)*U(3,1))*EQ(1,1)+(P(1,1)*U(3,2)-P(3,1)*U(1,2))*EQ(2,2);
EHatPvRUQt(3,1) = (P(1,3)*U(2,2)-P(2,3)*U(1,2))*EQ(2,2)+(P(2,2)*U(1,3)-P(1,2)*U(2,3))*EQ(3,3);
EHatPvRUQt(3,2) = (P(2,3)*U(1,1)-P(1,3)*U(2,1))*EQ(1,1)+(P(1,1)*U(2,3)-P(2,1)*U(1,3))*EQ(3,3);
EHatPvRUQt(3,3) = (P(1,2)*U(2,1)-P(2,2)*U(1,1))*EQ(1,1)+(P(2,1)*U(1,2)-P(1,1)*U(2,2))*EQ(2,2);
end
EHatPvRRt = EHatPvRUQt*V';
end
G = H*H';
EHW2 = (G-trace(G)*eye(3))*dt;
if omegaLocal
ERtdt = (ERt*(eye(3)+EHW2/2) + dt*ERHatPvRt)*expRot((omega+Miu)*dt);
else
ERtdt = expRot((omega+Miu)*dt)*(ERt*(eye(3)+EHW2/2) + dt*EHatPvRRt);
end
end