Explain of `CoreState::CalcQSmallAngleApprox`

extr15 commented 2 years ago

Hi authors, thanks for your work! I read the code and it is clear and well documented. There is a function CoreState::CalcQSmallAngleApprox in core_state_calc_q.cpp, could you explain how to generate it?

Chris-Bee commented 2 years ago

Hello extr15,

Thank you for your feedback and your question. We will add this to the documentation soon. CoreState::CalcQSmallAngleApprox() calculates the process noise Q as follows:

$\textbf{P}_{n+1}=\textbf{F}_x\textbf{P}\textbf{F}_x^\mathsf{T}+\textbf{Q}$ $\textbf{Q}=\int_{0}^{\Delta{}t}\textbf{F}_x\textbf{F}_n\textbf{P}\textbf{F}_n^\mathsf{T}\textbf{F}_x^\mathsf{T}dt$

with $\textbf{F}_x$ being the Jacobian of the state dynamics derived after the error states and $\textbf{F}_n$ being the Jacobian of the state dynamics derived after the propagation sensor noise.

At the very moment, this integration is done by formulating the problem symbolically in Matlab and integrating over $\Delta{}t$ . The resulting function is then exported to the C++ function that you mentioned. This function is unfortunately not directly human-readable. This will change in the next iteration.

However, and just for clarification: The core state propagation is agnostic to the update sensors. If you want to add a sensor module to MaRS then the process noise formulation remains the same.

extr15 commented 2 years ago

Hi Chris, thanks, and looking forward to your iteration.

aau-cns / mars_lib

Explain of `CoreState::CalcQSmallAngleApprox` #2