8、姿态估计与控制：理论、算法与实现

姿态估计与控制：理论、算法与实现

1. 姿态估计基础

在姿态估计中，首先定义了 $\Delta z_i$ 如下：
$\Delta z_i(t_k) = C_{BR}[q(t_k)] r_i^R - C_{B’R}[\hat{q}(t_k)] r_i^R = [(C_{B’R}[\hat{q}(t_k)]r_i^R)×] \delta\alpha$，其中 $i = 1, 2, \ldots, n$。由此可得：
$\Delta z_k =
\begin{bmatrix}
\Delta z_1(t_k) \
\Delta z_2(t_k) \
\vdots \
\Delta z_n(t_k)
\end{bmatrix}
=
\begin{bmatrix}
[(C_{B’R}[\hat{q}(t_k)]r_1^R)×] & 0_3 \
[(C_{B’R}[\hat{q}(t_k)]r_2^R)×] & 0_3 \
\vdots & \vdots \
[(C_{B’R}[\hat{q}(t_k)]r_n^R)×] & 0_3
\end{bmatrix}
\begin{bmatrix}
\delta\alpha(t_k) \
\delta\beta(t_k)
\end{bmatrix}
= H_k\Delta x_k$
这样就确定了 $H_k$ 的表达式。

线性化的状态空间形式和线性化的测量方程如下：
$\Delta x_{k + 1} = \Phi\Delta