fix derivation in section 4.2

tekrajchhetri · Oct 24, 2019 · 374f8d2 · 374f8d2
1 parent 951c980
commit 374f8d2
Showing 1 changed file with 18 additions and 17 deletions.
diff --git a/doc/doc.tex b/doc/doc.tex
@@ -699,34 +699,35 @@ \subsection{State augmentation with $g_{sb}$}
 \begin{aligned}
  & R_{bc}^\top\big(R_{sb}^\top(X_s-T_{sb})-T_{bc})\big) \\
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
- = & (\bar R_{bc} + \asym{\tilde\omega_{bc}})^\top
- \big( (\bar R_{sb} + \asym{\tilde\omega_{sb}})^\top (\bar X_s - \bar T_{sb}) + (\bar R_{sb} + \asym{\tilde\omega_{sb}})^\top (\tilde X_s - \tilde T_{sb}) - (\bar T_{bc} + \tilde T_{bc}) \big) \\
+ = & (\bar R_{bc} + \bar R_{bc}\asym{\tilde\omega_{bc}})^\top
+ \big( (\bar R_{sb} + \bar R_{sb}\asym{\tilde\omega_{sb}})^\top (\bar X_s - \bar T_{sb}) + (\bar R_{sb} + \bar R_{sb}\asym{\tilde\omega_{sb}})^\top (\tilde X_s - \tilde T_{sb}) - (\bar T_{bc} + \tilde T_{bc}) \big) \\
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
- = & (\bar R_{bc}^\top - \asym{\tilde\omega_{bc}})
- \big( (\bar R_{sb}^\top - \asym{\tilde\omega_{sb}}) (\bar X_s - \bar T_{sb}) + (\bar R_{sb}^\top - \asym{\tilde\omega_{sb}}) (\tilde X_s - \tilde T_{sb}) - (\bar T_{bc} + \tilde T_{bc}) \big) \\
+ = & (\bar R_{bc}^\top - \asym{\tilde\omega_{bc}} \bar R_{bc}^\top)
+ \big( (\bar R_{sb}^\top - \asym{\tilde\omega_{sb}} \bar R_{sb}^\top) (\bar X_s - \bar T_{sb}) + (\bar R_{sb}^\top - \asym{\tilde\omega_{sb}} \bar R_{sb}^\top) (\tilde X_s - \tilde T_{sb}) - (\bar T_{bc} + \tilde T_{bc}) \big) \\
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-= & (\bar R_{bc}^\top - \asym{\tilde\omega_{bc}})
- (\bar R_{sb}^\top - \asym{\tilde\omega_{sb}}) (\bar X_s - \bar T_{sb}) \\
- &+ (\bar R_{bc}^\top - \asym{\tilde\omega_{bc}})(\bar R_{sb}^\top - \asym{\tilde\omega_{sb}}) (\tilde X_s - \tilde T_{sb}) \\
- &- (\bar R_{bc}^\top - \asym{\tilde\omega_{bc}})(\bar T_{bc} + \tilde T_{bc}) \\
+= & (\bar R_{bc}^\top - \asym{\tilde\omega_{bc}} \bar R_{bc}^\top)
+ (\bar R_{sb}^\top - \asym{\tilde\omega_{sb}} \bar R_{sb}^\top) (\bar X_s - \bar T_{sb}) \\
+ &+ (\bar R_{bc}^\top - \asym{\tilde\omega_{bc}} \bar R_{bc}^
+ \top)(\bar R_{sb}^\top - \asym{\tilde\omega_{sb}} \bar R_{sb}^\top) (\tilde X_s - \tilde T_{sb}) \\
+ &- (\bar R_{bc}^\top - \asym{\tilde\omega_{bc}} \bar R_{bc}^\top)(\bar T_{bc} + \tilde T_{bc}) \\
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
- \approx & \bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb}) - \asym{\tilde \omega_{bc}}\bar R_{sb}^\top (\bar X_s - \bar T_{sb}) - \bar R_{bc}^\top \asym{\tilde \omega_{sb}}(\bar X_s - \bar T_{sb}) \\
+ \approx & \bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb}) - \asym{\tilde \omega_{bc}}\bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb}) - \bar R_{bc}^\top \asym{\tilde \omega_{sb}} \bar R_{sb}^\top (\bar X_s - \bar T_{sb}) \\
  &+ \bar R_{bc}^\top\bar R_{sb}^\top (\tilde X_s - \tilde T_{sb}) \\
  &- \bar R_{bc}^\top \bar T_{bc} - \bar R_{bc}^\top \tilde T_{bc} + \asym{\tilde \omega_{bc}} \bar T_{bc} \quad\text{(drop high-order terms)}\\
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  = & \bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb}) 
- + \asym{\bar R_{sb}^\top (\bar X_s - \bar T_{sb})}\tilde \omega_{bc} 
- + \bar R_{bc}^\top \asym{\bar X_s - \bar T_{sb}} \tilde \omega_{sb} \\
+ + \asym{\bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb})}\tilde \omega_{bc} 
+ + \bar R_{bc}^\top \asym{\bar R_{sb}^\top (\bar X_s - \bar T_{sb})} \tilde \omega_{sb} \\
  &+ \bar R_{bc}^\top\bar R_{sb}^\top (\tilde X_s - \tilde T_{sb}) \\
  &- \bar R_{bc}^\top \bar T_{bc} 
  - \bar R_{bc}^\top \tilde T_{bc} 
- - \asym{\bar T_{bc}}\tilde \omega_{bc}  \\
+ - \asym{\bar R_{bc}^\top \bar T_{bc}}\tilde \omega_{bc}  \\
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 =& \bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb}) - \bar R_{bc}^\top \bar T_{bc} \quad\text{(nominal term)}\\
-&+ \asym{\bar R_{sb}^\top (\bar X_s - \bar T_{sb})}\tilde \omega_{bc} 
-+ \bar R_{bc}^\top \asym{\bar X_s - \bar T_{sb}}\tilde \omega_{sb}
+&+ \asym{\bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb})}\tilde \omega_{bc} 
++ \bar R_{bc}^\top \asym{ \bar R_{sb}^\top (\bar X_s - \bar T_{sb})}\tilde \omega_{sb}
  + \bar R_{bc}^\top\bar R_{sb}^\top (\tilde X_s - \tilde T_{sb}) 
- - \bar R_{bc}^\top \tilde T_{bc} - \asym{\bar T_{bc}} \tilde \omega_{bc} \\
+ - \bar R_{bc}^\top \tilde T_{bc} - \asym{\bar R_{bc}^\top \bar T_{bc}} \tilde \omega_{bc} \\
 \end{aligned}
 \end{equation}
 
@@ -739,8 +740,8 @@ \subsection{State augmentation with $g_{sb}$}
     &\cdot 
     
     \big( 
-    \asym{\bar R_{sb}^\top (\bar X_s - \bar T_{sb}) - \bar T_{bc}}\tilde \omega_{bc} 
-+ \bar R_{bc}^\top \asym{\bar X_s - \bar T_{sb}}\tilde \omega_{sb}
+    \asym{\bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb}) - \bar R_{bc}^\top \bar T_{bc}}\tilde \omega_{bc} 
++ \bar R_{bc}^\top \asym{\bar R_{sb}^\top (\bar X_s - \bar T_{sb})}\tilde \omega_{sb}
 + \bar R_{bc}^\top\bar R_{sb}^\top (\tilde X_s - \tilde T_{sb}) 
 - \bar R_{bc}^\top \tilde T_{bc}
     \big)