KALMAN FILTER
Kalman filter is a recursive algorithm that uses dynamical system model and sensor readings for actual motion reconstruction. State vector assumption \({\mathbf{\hat {x}}}{{_{{}}^{ + }}_{{k - 1}}} = {\mathbf{\hat {x}}}\left( {{{t}_{k}}} \right)\) is calculated for each discrete time step \({{t}_{k}}\). Discrete Kalman filter utilizes correction of previous estimate. Consider step \(k - 1\) along with corresponding state vector estimation \({\mathbf{\hat {x}}}{{_{{}}^{ + }}_{{k - 1}}}\) and covariance matrix \({\mathbf{P}}_{{k - 1}}^{ + }\). The goal is to find the state vector estimate for the next step \({\mathbf{\hat {x}}}{{_{{}}^{ + }}_{k}}\). First a priory estimate \({\mathbf{\hat {x}}}{{_{{}}^{ - }}_{k}}\) is formed using straight mathematical model integration. It is corrected using the sensor measurements vector \({{{\mathbf{z}}}_{k}}\) to obtain a posteriori estimate \({\mathbf{\hat {x}}}{{_{{}}^{ + }}_{k}}\). Covariance error matrix \({\mathbf{P}}_{k}^{ - }\) is also constructed from the previous step information using Riccati equation. It is then updated to \({\mathbf{P}}_{k}^{ + }\) using measurements.
Kalman filter is designed for linear mathematical models and allows the best mean-square state vector estimate. It may be adapted for any non-linear mathematical models of both dynamical system and measurements,
$${\mathbf{\dot {x}}}\left( t \right) = {\mathbf{f}}\left( {{\mathbf{x}},t} \right) + {\mathbf{Gw}}\left( t \right),$$
$${\mathbf{\dot {z}}}\left( t \right) = {\mathbf{h}}\left( {{\mathbf{x}},t} \right) + {\mathbf{v}}\left( t \right),$$
where \({\mathbf{w}}\left( t \right)\) is a Gaussian dynamical model error with covariance matrix \({\mathbf{D}}\), \({\mathbf{G}}\) is a matrix of influence of model error on state vector, \({\mathbf{v}}\left( t \right)\) is a Gaussian measurements error with covariance matrix \({\mathbf{R}}\).
Kalman filter requires right-side functions \({\mathbf{f}}\left( {{\mathbf{x}},t} \right)\,\) and \({\mathbf{h}}\left( {{\mathbf{x}},t} \right)\) decomposition into the Taylor series in the vicinity of the current state vector. Only linear terms are used in the filter. Dynamical system and measurements model matrices are
$$\,{{{\mathbf{F}}}_{k}} = {{\left. {\frac{{\partial {\mathbf{f}}\left( {{\mathbf{x}},t} \right)}}{{\partial {\mathbf{x}}}}} \right|}_{{{\mathbf{x}} = {\mathbf{\hat {x}}}_{k}^{ - },\,t = {{t}_{k}}}}},\,\,\,\,{{{\mathbf{H}}}_{k}} = {{\left. {\frac{{\partial {\mathbf{h}}\left( {{\mathbf{x}},t} \right)}}{{\partial {\mathbf{x}}}}} \right|}_{{{\mathbf{x}} = {\mathbf{\hat {x}}}_{k}^{ - },\,t = {{t}_{k}}}}}.$$
Discrete extended Kalman filter uses non-linear dynamical and measurements models for a priory estimate prediction and a posteriori correction.
Prediction phase is
where \({{{\mathbf{Q}}}_{k}}\) is the covariance matrix of discrete-time process noise, it is calculated as
where 
is a transition matrix between states \(k - 1\) and \(k\). Correction phase is
$$\begin{gathered} {{{\mathbf{K}}}_{k}} = {\mathbf{P}}_{k}^{ - }{\mathbf{H}}_{k}^{T}{{\left( {{\mathbf{H}}_{k}^{{}}{\mathbf{P}}_{k}^{ - }{\mathbf{H}}_{k}^{T} + {{{\mathbf{R}}}_{k}}} \right)}^{{ - 1}}}, \hfill \\ {\mathbf{\hat {x}}}_{k}^{ + } = {\mathbf{\hat {x}}}_{k}^{ - } + {{{\mathbf{K}}}_{k}}\left( {{{{\mathbf{z}}}_{k}} - {\mathbf{h}}\left( {{\mathbf{\hat {x}}}_{k}^{ - },{{t}_{k}}} \right)} \right), \hfill \\ {\mathbf{P}}_{k}^{ + } = \left( {{\mathbf{E}} - {{{\mathbf{K}}}_{k}}{{{\mathbf{H}}}_{k}}} \right){\mathbf{P}}_{k}^{ - } \hfill \\ \end{gathered} $$
where \({\mathbf{E}}\) is an identity matrix, \({\mathbf{K}}\) is a weighing matrix.
We now construct Kalman filter to obtain satellite attitude in orbital reference frame. The state vector is
$${\mathbf{x}} = \left( {{\mathbf{q}},\,{\mathbf{\omega }}} \right).$$
Dynamical model of controlled satellite angular motion is (2), Gaussian disturbance is not taken into account. Quaternion kinematics (6) is used.
Equations of motion should be linearized in the vicinity of current state vector. Rewrite equations (2), (6) as
$$\delta {\mathbf{\dot {x}}}\left( t \right) = {\mathbf{F}}\left( t \right)\delta {\mathbf{x}}\left( t \right),$$
where \(\delta {\mathbf{x}}\left( t \right)\) is a small state vector increment, \({\mathbf{F}}\left( t \right)\) is the matrix of equations of motion linearized in the vicinity of current state. State vector \({\mathbf{x}}\left( t \right)\) can be divided into the estimated part \({\mathbf{\hat {x}}}\left( t \right)\) and misalignment \(\delta {\mathbf{x}}\left( t \right)\),
$${\mathbf{x}}\left( t \right) = {\mathbf{\hat {x}}}\left( t \right) + \delta {\mathbf{x}}\left( t \right).$$
(А.1)
In order to linearize kinematic relations (6) note that quaternion sum in (A.1) actually means sum of rotations. This is represented by quaternion multiplication in Rodrigo-Hamilton parameters
$$\Lambda = \hat {\Lambda } \circ \delta \Lambda .$$
Here \(\hat {\Lambda }\) is the current estimated quaternion. Omitting second order small terms, terms of the order of orbital velocity and assuming unit scalar part for \(\delta \Lambda = \left( {\delta {\mathbf{q}},\,\,\,1} \right)\) we get linearized kinematic relations
$$\delta {\mathbf{\dot {q}}} = {{{\mathbf{W}}}_{{\mathbf{\omega }}}}\delta {\mathbf{q}} + \frac{1}{2}\delta {\mathbf{\omega }},\,\,\,\,\delta {{\dot {q}}_{0}} = 0.$$
(А.2)
Linearization of equations (2) involves gravitational torque (5)
$${{{\mathbf{M}}}_{{{\text{gr}}}}} = 3\omega _{0}^{2}{\mathbf{A}}\left( \Lambda \right){{{\mathbf{e}}}_{3}} \times {\mathbf{JA}}\left( \Lambda \right){{{\mathbf{e}}}_{3}}.$$
We use small rotation in the vicinity of current estimate,
$${\mathbf{A}}\left( \Lambda \right) = {\mathbf{A}}\left( {\hat {\Lambda } \circ \delta \Lambda } \right) = {\mathbf{A}}\left( {\delta \Lambda } \right){\mathbf{A}}\left( {\hat {\Lambda }} \right).$$
(А.3)
This leads to
$$\delta {{{\mathbf{M}}}_{{{\text{gr}}}}} = 3\omega _{0}^{2}\left( {{{{\mathbf{W}}}_{{{{{\mathbf{e}}}_{3}}}}}{\mathbf{J}}{{{\mathbf{W}}}_{{{{{\mathbf{e}}}_{3}}}}} - {{{\mathbf{W}}}_{{{\mathbf{J}}{{{\mathbf{e}}}_{3}}}}}{{{\mathbf{W}}}_{{{{{\mathbf{e}}}_{3}}}}}} \right)\delta {\mathbf{q}}.$$
Finally linearized gravitational torque is
$$\delta {{{\mathbf{M}}}_{{{\text{gr}}}}} = 6\omega _{0}^{2}{{{\mathbf{F}}}_{{{\text{gr}}}}}\delta {\mathbf{q}},$$
(А.4)
where
$${{{\mathbf{F}}}_{{{\text{gr}}}}} = {{{\mathbf{W}}}_{{{{{\mathbf{e}}}_{3}}}}}{\mathbf{J}}{{{\mathbf{W}}}_{{{{{\mathbf{e}}}_{3}}}}} - {{{\mathbf{W}}}_{{{\mathbf{J}}{{{\mathbf{e}}}_{3}}}}}{{{\mathbf{W}}}_{{{{{\mathbf{e}}}_{3}}}}},$$
\({{e}_{x}},{{e}_{y}},{{e}_{z}}\) are unit radius-vector components in the bound frame.
Linearized gyroscopic torque is
$$\delta {{{\mathbf{M}}}_{{{\text{gir}}}}} = {{{\mathbf{F}}}_{{{\text{gir}}}}}\delta {\mathbf{\omega }},$$
(А.5)
where
$${{{\mathbf{F}}}_{{gir}}} = \left( {\begin{array}{*{20}{c}} 0&{{{\omega }_{3}}\left( {B - C} \right)}&{{{\omega }_{2}}\left( {B - C} \right)} \\ {{{\omega }_{3}}\left( {C - A} \right)}&0&{{{\omega }_{1}}\left( {C - A} \right)} \\ {{{\omega }_{2}}\left( {A - B} \right)}&{{{\omega }_{1}}\left( {A - B} \right)}&0 \end{array}} \right).$$
$$\frac{d}{{dt}}\delta {\mathbf{x}}\left( t \right) = {\mathbf{F}}\left( t \right)\delta {\mathbf{x}}\left( t \right),$$
where dynamics matrix \({\mathbf{F}}\) is introduced using (A.2), (A.4), and (A.5) as
$${\mathbf{F}} = \left( {\begin{array}{*{20}{c}} {{{{\mathbf{W}}}_{{\mathbf{\omega }}}}}&{\frac{1}{2}{\mathbf{E}}} \\ {{{{\mathbf{J}}}^{{ - 1}}}\left( {6\omega _{0}^{2}{{{\mathbf{F}}}_{{{\text{gr}}}}} - {{k}_{a}}{\mathbf{E}}} \right)}&{ - {{{\mathbf{J}}}^{{ - 1}}}{{k}_{\omega }} + {{{\mathbf{J}}}^{{ - 1}}}{{{\mathbf{F}}}_{{{\text{gir}}}}}} \end{array}} \right).$$
Magnetometer measurements model is
$${\mathbf{z}} = \left[ {\begin{array}{*{20}{c}} {{\mathbf{A}}{{{\mathbf{B}}}_{{{\text{orb}}}}} + {{{\mathbf{\eta }}}_{{\mathbf{B}}}}} \\ {{\mathbf{\omega }} + {{{\mathbf{\eta }}}_{{\mathbf{\omega }}}}} \end{array}} \right],$$
(А.6)
where \({{{\mathbf{\eta }}}_{{\mathbf{B}}}}\) is a Gaussian geomagnetic induction vector error with zero mean. Linearized measurements model is
$${\mathbf{z}} = {\mathbf{\hat {z}}} + \delta {\mathbf{z}} = \left[ {\begin{array}{*{20}{c}} {{\mathbf{A}}\left( {\hat {\Lambda } \circ \delta \Lambda } \right){{{\mathbf{B}}}_{{{\text{orb}}}}}} \\ {{\mathbf{\hat {\omega }}} + \delta {\mathbf{\omega }}} \end{array}} \right].$$
Using (A.3) and (A.6) we get
$$\delta {\mathbf{z}} = \left[ {\begin{array}{*{20}{c}} { - 2{{{\mathbf{W}}}_{{\delta {\mathbf{q}}}}}{\mathbf{\hat {B}}}} \\ {\delta {\mathbf{\omega }}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} { - 2{{{\mathbf{W}}}_{{{\mathbf{\hat {B}}}}}}\delta {\mathbf{q}}} \\ {\delta {\mathbf{\omega }}} \end{array}} \right].$$
and measurements matrix is
$${\mathbf{H}} = \left[ {\begin{array}{*{20}{c}} { - 2{{{\mathbf{W}}}_{{{\mathbf{\hat {B}}}}}}}&{{{0}_{{3 \times 3}}}} \\ {{{0}_{{3 \times 3}}}}&{{{{\mathbf{E}}}_{{3 \times 3}}}} \end{array}} \right].$$