1\documentclass{article}
2
3%\usepackage[pdftex]{graphicx}
4\usepackage[T1]{fontenc}
5\usepackage{ae,aecompl}
6\usepackage{graphicx}
7\usepackage{psfrag}
8\usepackage{bm, amsmath, amsfonts, amssymb}
9%\usepackage[dvips,breaklinks=true,colorlinks=false]{hyperref}
10\usepackage{html}
11\usepackage{comment}
12
13\input{../../manual/stdmacro}
14%poor man's bold symbol
15\newcommand{\T}[1]{\bm{#1}}
16\newcommand{\TT}[1]{\bm{#1}}
17\newcommand{\TTT}[1]{\bm{#1}}
18\newcommand{\TTTT}[1]{\mathbb{#1}}
19\newcommand{\equu}{\overset{\text{uu}}{=}}
20\newcommand{\dof}{DOF}
21\newcommand{\dofs}{DOFs}
22
23% Custom
24\topmargin 0.31cm % 0.00cm
25\oddsidemargin 0.00cm
26\evensidemargin 0.00cm
27\marginparsep 0.00cm
28\textwidth 15.92cm
29\textheight 21.00cm % 23.62cm
30
31
32\begin{document}
33
34\title{Module IMU}
35\author{Pierangelo Masarati \\ \texttt{masarati@aero.polimi.it}}
36\date{\today}
37\maketitle
38
39
40\section{Introduction}
41\begin{subequations}
42\begin{align}
43	\TT{R}
44	&=
45	\TT{R}_n \tilde{\TT{R}}_h
46	\\
47	\T{x}
48	&=
49	\T{x}_n
50	+
51	\TT{R}_n \tilde{\T{f}}
52\end{align}
53\end{subequations}
54
55\begin{align}
56	\T{f}
57	&=
58	\TT{R}_n \tilde{\T{f}}
59\end{align}
60
61\begin{align}
62	\overline{(\cdot)}
63	&=
64	\TT{R}^T (\cdot)
65\end{align}
66
67\section{Angular Velocity Measurement}
68\begin{align}
69	\T{\omega}\times{}
70	&=
71	\dot{\TT{R}}\TT{R}^T
72	=
73	\T{\omega}_n\times{}
74\end{align}
75
76\begin{align}
77	\overline{\T{\omega}}
78	&=
79	\TT{R}^T \T{\omega}_n
80\end{align}
81
82\section{Linear Acceleration Measurement}
83\begin{align}
84	\dot{\T{x}}
85	&=
86	\dot{\T{x}}_n
87	+
88	\T{\omega}_n \times \T{f}
89\end{align}
90
91\begin{align}
92	\overline{\dot{\T{x}}}
93	&=
94	\TT{R}^T \dot{\T{x}}_n
95	+
96	\plbr{\TT{R}^T \T{\omega}_n} \times \overline{\T{f}}
97	=
98	\overline{\T{v}}
99\end{align}
100
101\begin{align}
102	\ddot{\T{x}}
103	&=
104	\ddot{\T{x}}_n
105	+
106	\dot{\T{\omega}} \times \T{f}
107	+
108	\T{\omega}_n \times \T{\omega}_n \times \T{f}
109\end{align}
110
111\begin{align}
112	\overline{\ddot{\T{x}}}
113	&=
114	\TT{R}^T \ddot{\T{x}}_n
115	+
116	\plbr{\TT{R}^T \dot{\T{\omega}}} \times \overline{\T{f}}
117	+
118	\plbr{\TT{R}^T \T{\omega}} \times \plbr{\TT{R}^T \T{\omega}} \times \overline{\T{f}}
119\end{align}
120
121\begin{align}
122	\overline{\T{f}}
123	&=
124	\TT{R}^T \T{f}
125	=
126	\tilde{\TT{R}}_h^T \tilde{\T{f}}
127\end{align}
128
129\section{IMU Constraint}
130Constraint equations
131\begin{subequations}
132\begin{align}
133	\TT{R}^T \T{\omega}_n - \overline{\T{\omega}}
134	&=
135	\T{0}
136	\\
137	\TT{R}^T \dot{\T{x}}_n
138	+
139	\plbr{\TT{R}^T \T{\omega}_n} \times \overline{\T{f}}
140	-
141	\overline{\T{v}}
142	&=
143	\T{0}
144	\\
145	\dot{\overline{\T{v}}}
146	-
147	\overline{\T{a}}
148	&=
149	\T{0}
150\end{align}
151\end{subequations}
152
153Linearization of constraint equations
154\begin{subequations}
155\begin{align}
156	\TT{R}^T \plbr{
157		\delta\T{\omega}_n
158		+
159		\T{\omega}_n \times \T{\theta}_{n\delta}
160	}
161	&\equu
162	\TT{R}^T \delta\dot{\T{g}}
163	\\
164	\TT{R}^T \plbr{
165		\delta\dot{\T{x}}_n
166		+
167		\dot{\T{x}}_n \times \T{\theta}_{n\delta}
168	}
169	+
170	\plbr{
171		\TT{R}^T \plbr{
172			\delta\T{\omega}_n
173			+
174			\T{\omega}_n \times \T{\theta}_{n\delta}
175		}
176	} \times \overline{\T{f}}
177	-
178	\delta\overline{\T{v}}
179	&\equu
180	\TT{R}^T \plbr{
181		\delta\dot{\T{x}}_n
182		+
183		\dot{\T{x}}_n \times \delta\T{g}_n
184		-
185		\T{f} \times \delta\dot{\T{g}}_n
186	}
187	-
188	\delta\overline{\T{v}}
189	\\
190	\delta\dot{\overline{\T{v}}}
191	&\equu
192	\delta\dot{\overline{\T{v}}}
193\end{align}
194\end{subequations}
195
196Forces and moments
197\begin{subequations}
198\begin{align}
199	\T{f}_n
200	&=
201	\TT{R} \T{\lambda}_{\T{v}}
202	\\
203	\T{m}_n
204	&=
205	\TT{R} \T{\lambda}_{\T{\omega}}
206	+
207	\T{f} \times \TT{R} \T{\lambda}_{\T{v}}
208\end{align}
209\end{subequations}
210
211Linearization of forces and moments
212\begin{subequations}
213\begin{align}
214	\delta\T{f}_n
215	&=
216	\TT{R} \delta \T{\lambda}_{\T{v}}
217	- \plbr{\TT{R} \T{\lambda}_{\T{v}}} \times \T{\theta}_{n\delta}
218	\\
219	\delta \T{m}_n
220	&=
221	\TT{R} \delta \T{\lambda}_{\T{\omega}}
222	+
223	\T{f} \times \TT{R} \delta \T{\lambda}_{\T{v}}
224	-
225	\plbr{
226		\TT{R} \T{\lambda}_{\T{\omega}}
227		+
228		\T{f} \times \TT{R} \T{\lambda}_{\T{v}}
229	} \times \T{\theta}_{n\delta}
230\end{align}
231\end{subequations}
232
233$\T{\lambda}_{\T{v}}$, $\T{\lambda}_{\T{\omega}}$: Lagrange's multipliers (algebraic);
234$\overline{\T{v}}$: local velocity (differential).
235
236\begin{align}
237	\sqbr{\matr{cc|cc|c}{
238		\TT{0} & -c\plbr{\TT{R} \T{\lambda}_{\T{v}}}\times{}
239			& \TT{R} & \TT{0} & \TT{0}
240		\\
241		\TT{0} & -c\plbr{\TT{R} \T{\lambda}_{\T{\omega}} + \T{f}\times\TT{R} \T{\lambda}_{\T{v}}}\times{}
242			& \T{f}\times\TT{R} & \TT{R} & \TT{0}
243		\\
244		\hline
245		\TT{R}^T & \TT{R}^T \plbr{\T{f}\times{}^T - c \dot{\T{x}}_n\times{}^T}
246			& \TT{0} & \TT{0} & -c \TT{I}
247		\\
248		\TT{0} & \TT{R}^T & \TT{0} & \TT{0} & \TT{0}
249		\\
250		\hline
251		\TT{0} & \TT{0} & \TT{0} & \TT{0} & \TT{I}
252	}} \cubr{\cvvect{
253		\delta\dot{\T{x}}_n
254		\\
255		\delta\dot{\T{g}}_n
256		\\
257		\delta \T{\lambda}_{\T{v}}
258		\\
259		\delta \T{\lambda}_{\T{\omega}}
260		\\
261		\delta\overline{\T{v}}
262	}}
263\end{align}
264
265
266\end{document}
267