数式

$ x = \left[ \begin{array}{r} \theta \\ \dot{\theta} \\ \dot{\theta}_{b} \\ \end{array} \right] $

$ \begin{cases} \theta_{k} = \theta_{k-1} + \Delta t (\dot{\theta}_{k-1} - \dot{\theta}_{b_{k-1}}) \\ \dot{\theta_{k}} = \dot{\theta_{k-1}} \\ \dot{\theta}_{b_{k-1}} = \dot{\theta}_{b_{k}} \end{cases} $

$ x_{k} = F x_{k-1} + B u_{k-1} + w_{k} $

$ F = \left[ \begin{array}{rrr} 1 & \Delta t & \Delta t \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{array} \right] $

$ B = 0 $

$ p(\mathbf{w}) =N(0,Q) $

$ Q = \left[ \begin{array}{rrr} q_{1} & 0 & 0 \\ 0 & q_{2} & 0 \\ 0 & 0 & q_{3} \end{array} \right] $

$ P_{k}=F P_{k-1} F^{T} $

$ y_{k} = z_{k} - H x_{k} + v_{k} $

$ z = \left[ \begin{array}{r} \theta \\ \dot{\theta} \\ \end{array} \right] $

参考

http://www.linushelgesson.se/2012/04/pitch-and-roll-estimating-kalman-filter-for-stabilizing-quadrocopters/