QCopter
EstimatorModel
Search
Search:
Navigation
Welcome
Java
AVRLib
QCopter/EstimatorModel
User
Login
Impressum
MoinMoin Powered
Python Powered
Valid HTML 4.01
Model Based State Estimator
State and input vectors
Parameters
Conservation of linear and angular momentum
State space equation
QCopter/EstimatorModel (last edited 2009-01-23 17:45:34 by
StefanEngelke
)
![$x = \left[\begin{array}{c} \phi_p\\ \phi_r\\ \omega_p\\ \omega_r \\ \omega_y \\ v_z \\ b_p \\ b_r \\ b_y \end{array}\right] \begin{array}{l}\rm{pitch~angle}\\\rm{roll~angle}\\\rm{pitch~velocity}\\\rm{roll~velocity}\\\rm{yaw~velocity}\\\rm{translation~velocity~in~z~direction}\\\rm{bias~of~pitch~gyro}\\\rm{bias~of~roll~gyro}\\\rm{bias~of~yaw~gyro}\end{array}\quad\,$](/QCopter/EstimatorModel?action=AttachFile&do=get&target=latex_b7c7827b75ee62777e6da549afe7414976eea6e0_p1.png "$x = \left[\begin{array}{c} \phi_p\\ \phi_r\\ \omega_p\\ \omega_r \\ \omega_y \\ v_z \\ b_p \\ b_r \\ b_y \end{array}\right] \begin{array}{l}\rm{pitch~angle}\\\rm{roll~angle}\\\rm{pitch~velocity}\\\rm{roll~velocity}\\\rm{yaw~velocity}\\\rm{translation~velocity~in~z~direction}\\\rm{bias~of~pitch~gyro}\\\rm{bias~of~roll~gyro}\\\rm{bias~of~yaw~gyro}\end{array}\quad\,$")
![$u = \left[\begin{array}{c}F_f\\F_b\\F_r\\F_l\end{array}\right] \begin{array}{l}\rm{front~thrust}\\\rm{rear~thrust}\\\rm{right~thrust}\\\rm{left~thrust}\end{array}$](/QCopter/EstimatorModel?action=AttachFile&do=get&target=latex_dc213e3afc9486f935c2a545165abb8a9663b5e5_p1.png "$u = \left[\begin{array}{c}F_f\\F_b\\F_r\\F_l\end{array}\right] \begin{array}{l}\rm{front~thrust}\\\rm{rear~thrust}\\\rm{right~thrust}\\\rm{left~thrust}\end{array}$")
![$\underbrace{\frac{\partial}{\partial t} \left[\begin{array}{c} \phi_p\\ \phi_r\\ \omega_p\\ \omega_r \\ \omega_y \\ v_z \\ b_p \\ b_r \\ b_y \end{array}\right]}_{\dot{x}} = \underbrace{\left[\begin{array}{ccccccccc} 0&0&1&0&0&0&0&0&0 \\ 0&0&0&1&0&0&0&0&0 \\ 0&0&0&0&0&0&0&0&0 \\ 0&0&0&0&0&0&0&0&0 \\ 0&0&0&0&0&0&0&0&0 \\ 0&0&0&0&0&0&0&0&0 \\ 0&0&0&0&0&0&0&0&0 \\ 0&0&0&0&0&0&0&0&0 \\ 0&0&0&0&0&0&0&0&0 \end{array}\right]}_{A} \underbrace{\left[\begin{array}{c} \phi_p\\ \phi_r\\ \omega_p\\ \omega_r \\ \omega_y \\ v_z \\ b_p \\ b_r \\ b_y \end{array}\right]}_{x} + \underbrace{\left[\begin{array}{cccc} 0&0&0&0 \\ 0&0&0&0 \\ -\frac{\ell}{\Phi_p}&\frac{\ell}{\Phi_p}&0&0 \\ 0&0&-\frac{\ell}{\Phi_r}&\frac{\ell}{\Phi_r} \\ \frac{\gamma}{\Phi_y}&\frac{\gamma}{\Phi_y}&-\frac{\gamma}{\Phi_y}&-\frac{\gamma}{\Phi_y} \\ \frac{1}{m}&\frac{1}{m}&\frac{1}{m}&\frac{1}{m} \\ 0&0&0&0 \\ 0&0&0&0 \\ 0&0&0&0 \end{array}\right]}_{B} \underbrace{\left[\begin{array}{c}F_f\\F_b\\F_r\\F_l\end{array}\right]}_{u} $](/QCopter/EstimatorModel?action=AttachFile&do=get&target=latex_086241a2c397aa996aaebbac99a5626e5933137d_p1.png "$\underbrace{\frac{\partial}{\partial t} \left[\begin{array}{c} \phi_p\\ \phi_r\\ \omega_p\\ \omega_r \\ \omega_y \\ v_z \\ b_p \\ b_r \\ b_y \end{array}\right]}_{\dot{x}} = \underbrace{\left[\begin{array}{ccccccccc} 0&0&1&0&0&0&0&0&0 \\ 0&0&0&1&0&0&0&0&0 \\ 0&0&0&0&0&0&0&0&0 \\ 0&0&0&0&0&0&0&0&0 \\ 0&0&0&0&0&0&0&0&0 \\ 0&0&0&0&0&0&0&0&0 \\ 0&0&0&0&0&0&0&0&0 \\ 0&0&0&0&0&0&0&0&0 \\ 0&0&0&0&0&0&0&0&0 \end{array}\right]}_{A} \underbrace{\left[\begin{array}{c} \phi_p\\ \phi_r\\ \omega_p\\ \omega_r \\ \omega_y \\ v_z \\ b_p \\ b_r \\ b_y \end{array}\right]}_{x} + \underbrace{\left[\begin{array}{cccc} 0&0&0&0 \\ 0&0&0&0 \\ -\frac{\ell}{\Phi_p}&\frac{\ell}{\Phi_p}&0&0 \\ 0&0&-\frac{\ell}{\Phi_r}&\frac{\ell}{\Phi_r} \\ \frac{\gamma}{\Phi_y}&\frac{\gamma}{\Phi_y}&-\frac{\gamma}{\Phi_y}&-\frac{\gamma}{\Phi_y} \\ \frac{1}{m}&\frac{1}{m}&\frac{1}{m}&\frac{1}{m} \\ 0&0&0&0 \\ 0&0&0&0 \\ 0&0&0&0 \end{array}\right]}_{B} \underbrace{\left[\begin{array}{c}F_f\\F_b\\F_r\\F_l\end{array}\right]}_{u} $")
![$\underbrace{\left[\begin{array}{c} g_p\\g_r\\g_y\\\hat{b}\\ \hat{\phi}_p\\ \hat{\phi}_r\\\end{array}\right]}_{y} = \underbrace{\left[ \begin{array}{ccccccccc} 0&0&1&0&0&0&1&0&0 \\ 0&0&0&1&0&0&0&1&0 \\ 0&0&0&0&1&0&0&0&1 \\ 0&0&0&0&0&0&1&1&1 \\ 1&0&0&0&0&0&0&0&0 \\ 0&1&0&0&0&0&0&0&0 \end{array}\right]}_{C} \underbrace{\left[\begin{array}{c} \phi_p\\ \phi_r\\ \omega_p\\ \omega_r \\ \omega_y \\ v_z \\ b_p \\ b_r \\ b_y \end{array}\right]}_{x}$](/QCopter/EstimatorModel?action=AttachFile&do=get&target=latex_61f543b02f89f384d9e043a1abcca11efab0185f_p1.png "$\underbrace{\left[\begin{array}{c} g_p\\g_r\\g_y\\\hat{b}\\ \hat{\phi}_p\\ \hat{\phi}_r\\\end{array}\right]}_{y} = \underbrace{\left[ \begin{array}{ccccccccc} 0&0&1&0&0&0&1&0&0 \\ 0&0&0&1&0&0&0&1&0 \\ 0&0&0&0&1&0&0&0&1 \\ 0&0&0&0&0&0&1&1&1 \\ 1&0&0&0&0&0&0&0&0 \\ 0&1&0&0&0&0&0&0&0 \end{array}\right]}_{C} \underbrace{\left[\begin{array}{c} \phi_p\\ \phi_r\\ \omega_p\\ \omega_r \\ \omega_y \\ v_z \\ b_p \\ b_r \\ b_y \end{array}\right]}_{x}$")
