update ImuFactor doc
							parent
							
								
									10a73338da
								
							
						
					
					
						commit
						755c752782
					
				|  | @ -1427,15 +1427,34 @@ pose/velocity/bias | |||
| 
 | ||||
| \begin_layout Standard | ||||
| We expand the state vector as  | ||||
| \begin_inset Formula $\zeta_{k}=[\theta_{k},p_{k},v_{k},a_{k}^{b}, \omega_{k}^{b}]$ | ||||
| \begin_inset Formula $\zeta_{k}=[\theta_{k},p_{k},v_{k},b_{k}^{a},b_{k}^{\omega}]$ | ||||
| \end_inset | ||||
| 
 | ||||
| . | ||||
|  For the jacobian  | ||||
|  to include the bias terms. | ||||
|  This gives the noise propagation equation as | ||||
| \end_layout | ||||
| 
 | ||||
| \begin_layout Standard | ||||
| \begin_inset Formula  | ||||
| \begin{equation} | ||||
| \Sigma_{k+1}=F_{k}\Sigma_{k}F_{k}^{T}+G_{k}\Sigma_{k}G_{k}\label{eq:prop-combined} | ||||
| \end{equation} | ||||
| 
 | ||||
| \end_inset | ||||
| 
 | ||||
| 
 | ||||
| \end_layout | ||||
| 
 | ||||
| \begin_layout Standard | ||||
| where  | ||||
| \begin_inset Formula $F_{k}$ | ||||
| \end_inset | ||||
| 
 | ||||
|  of  | ||||
|  is the  | ||||
| \begin_inset Formula $15\times15$ | ||||
| \end_inset | ||||
| 
 | ||||
|  derivative of  | ||||
| \begin_inset Formula $f$ | ||||
| \end_inset | ||||
| 
 | ||||
|  | @ -1443,13 +1462,21 @@ We expand the state vector as | |||
| \begin_inset Formula $\zeta$ | ||||
| \end_inset | ||||
| 
 | ||||
| , we get a  | ||||
| , and  | ||||
| \begin_inset Formula $G_{k}$ | ||||
| \end_inset | ||||
| 
 | ||||
|  is the  | ||||
| \begin_inset Formula $15\times15$ | ||||
| \end_inset | ||||
| 
 | ||||
|  matrix. | ||||
|  matrix for first order uncertainty propagation. | ||||
|  The top-left  | ||||
| \begin_inset Formula $9\times9$ | ||||
| \end_inset | ||||
| 
 | ||||
|  of  | ||||
| \begin_inset Formula $F_{k}$ | ||||
| \end_inset | ||||
| 
 | ||||
|  is the same as  | ||||
|  | @ -1481,7 +1508,7 @@ derivation as matrices | |||
| \begin_layout Standard | ||||
| \begin_inset Formula  | ||||
| \[ | ||||
| F_{k}=\left[\begin{array}{ccccc} | ||||
| F_{k}\approx\left[\begin{array}{ccccc} | ||||
| I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}} &  &  &  & H(\theta_{k})^{-1}\Delta_{t}\\ | ||||
| R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\frac{\Delta_{t}}{2}^{2} & I_{3\times3} & I_{3\times3}\Delta_{t} & R_{k}\frac{\Delta_{t}}{2}^{2}\\ | ||||
| R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} &  & I_{3\times3} & R_{k}\Delta_{t}\\ | ||||
|  |  | |||
										
											Binary file not shown.
										
									
								
							
		Loading…
	
		Reference in New Issue