## Part 1 - Mathematical Model

### (1)State space
>To define the model equation like:
$$\dot x_t=f_t(x_t)+G_t(x_t)w_t \\
z_{k+1}=h_{k+1}(x_{k+1})+V_{k+1}$$

$x^T=[q^T, \mu^T]$,
where $x$ is a $7\times1$ vector.

#### (1.1)For $q$ state space

With: $\dot q=\frac{1}{2}\Omega(w_t^m)q$,
where $w_t^m=w_t+\mu_t+\epsilon_t$.

So as for the state space equation, we get:
$\dot q=\frac{1}{2}\Omega(w_t+\mu_t+\epsilon_t)q$

Hence $\dot q=\frac{1}{2}\Omega(w_t+\mu_t)q+\frac{1}{2}\Omega(\epsilon_t)q$
We's like to regard $\dot q$ as two parts:
- state space model part - $\frac{1}{2}\Omega(w_t+\mu_t)q$
- noise part - $\frac{1}{2}\Omega(\epsilon_t)q$.

For noise part, we can write it in another form: $\frac{1}{2}\Xi \epsilon_t$, which is formed by the matrix mutiplying the noise part.

#### (1.2)For $\mu$ state space
With no doubt:
$\dot\mu=n_t$
It also contains 2 part:
- state space part - $0$
- noise part - $n_t$

The standard form of state space is $\dot x_t=f_t(x_t)+G_t(x_t)w_t$, among which :

$$f(x)=\frac{1}{2}\
\begin{bmatrix}
0&\omega_3-\mu_3&-\omega_2+\mu_2&\omega_1-\mu_1&0&0&0 \\
-\omega_3+\mu_3&0&\omega_1-\mu_1&\omega_2-\mu_2&0&0&0 \\
\omega_2-\mu_2&-\omega_1+\mu_1&0&\omega_3-\mu_3&0&0&0 \\
-\omega_1+\mu_1&-\omega_2+\mu_2&-\omega_3+\mu_3&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{bmatrix}
\begin{bmatrix}
e_1 \\
e_2 \\
e_3 \\
q \\
\mu_1 \\
\mu_2 \\
\mu_3
\end{bmatrix}$$
And:
$$G_x(x)=-\frac{1}{2}\begin{bmatrix}
q&-e_3&e_2&0&0&0 \\
e_3&q&-e_1&0&0&0 \\
-e_2&e_1&q&0&0&0 \\
e_1&e_2&e_3&0&0&0 \\
0&0&0&2&0&0 \\
0&0&0&0&2&0 \\
0&0&0&0&0&2
\end{bmatrix}$$
And also:

$$\sigma_x=\begin{bmatrix}
\epsilon_1 \\
\epsilon_2 \\
\epsilon_3 \\
n_1 \\
n_2 \\
n_3
\end{bmatrix}$$

The form of measurement equation is $z_{k+1} = h_{k+1}(x_{k+1} ) + v_{k+1}$

It is easy to deduce that:

$z^T=[b]$, which is a $3\times1$ vector.

$$\begin{bmatrix}
b
\end{bmatrix}=\begin{bmatrix}
A
\end{bmatrix}\begin{bmatrix}
r \\
\end{bmatrix}$$

$A$ is a 3 by 3 vector in the form of $A(\vec q)=(q^2-e^Te)I_3+2ee^T-2q[e\times]$
$$v_{k+1}=\delta b_{k+1}$$

$$[e\times]=\begin{bmatrix}
0 & -e_3 & e_2 \\
e_3 & 0 & -e_1 \\
-e_2 & e_1 & 0
\end{bmatrix}$$

**Linearization**

$F_x(x)$ is the partial derivative (Also a 7 by 7 Jacobi matrix) of $f(x)$ with regard of x:

$$F_x(x)=\frac{1}{2}\
\begin{bmatrix}
0&\omega_3-\mu_3&-\omega_2+\mu_2&\omega_1-\mu_1&-q&e_3&-e_2 \\
-\omega_3+\mu_3&0&\omega_1-\mu_1&\omega_2-\mu_2&-e_3&-q&e_1 \\
\omega_2-\mu_2&-\omega_1+\mu_1&0&\omega_3-\mu_3&e_2&-e_1&-q \\
-\omega_1+\mu_1&-\omega_2+\mu_2&-\omega_3+\mu_3&0&e_1&e_2&e_3 \\
0&0&0&0&0&0&0 \\
0&0&0&0&0&0&0 \\
0&0&0&0&0&0&0 \\
\end{bmatrix}$$

$G_x(x)$ is the partial derivative (Also a 7 by 7 Jacobi matrix) of $f(x)$ with regard of $\sigma_t$:

$$G_x(x)=-\frac{1}{2}\begin{bmatrix}
q&-e_3&e_2&0&0&0 \\
e_3&q&-e_1&0&0&0 \\
-e_2&e_1&q&0&0&0 \\
e_1&e_2&e_3&0&0&0 \\
0&0&0&2&0&0 \\
0&0&0&0&2&0 \\
0&0&0&0&0&2
\end{bmatrix}$$

$H_x(x)$ is the partial derivative (Also a 3 by 7 Jacobi matrix) of $h(x)$ with regard of x:

$$H_x(x)=\frac{\partial A}{\partial q}=
2\begin{bmatrix}
e^TrI_3+er^T-re^T+2q[r\times] &: &qe+[r\times]e
\end{bmatrix}$$

**Discretization**
$F_k=I_7+\delta_*F_x(x_k)$

### (2)Discrete-time linear perturbation model

To write down the discreate linear perturbation model equations in form of
$\delta x_{k+1}= F_x(x_k^* )\delta x_k + G(x_k^*) w_k$
$\delta z_{k+1} = H_x(x_{k+1}^*) \delta x_k + v_{k+1}$

Where $\delta x=[\delta e_1,\delta e_2,\delta e_3, \delta \mu_1,\delta \mu_2,\delta \mu_3]^T$

And :
$F_x(x_k^* )=\begin{bmatrix}
-w\times& I_3\\
0& 0
\end{bmatrix}$, which is a 6 by 6 matrix.
$G(x^*_k)=\frac{1}{2}I_6$
$H_x(x_{k+1}^*)=[-b\times, 0]$, which is a 3 by 6 matrix.

## Part 2

### (1)AEKF1

**Initialize step**
- Choose $P_{0|0},x_{0|0}$
- Set $R=\begin{bmatrix}
\sigma^2_b
\end{bmatrix}I_3$
- Set $Q=\begin{bmatrix}
\sigma^2_{\epsilon} \\
\sigma^2_n
\end{bmatrix}I_6$

**Predict step**
- $x_{k|k-1}=x_{k-1|k-1}+\delta tf(x_{k-1|k-1})$ where $w=w^m-\mu$
- $P_{k|k-1}=F_kP_{k-1|k-1}F_k^T+G_k(x_{k-1|k-1})QG_k^T(x_{k-1|k-1})$with star sensor frequency
Among which:
$F_k=\delta tF_x(x_{k-1|k-1})+I_7$

**Update for star sensor:**
- $K_k=P_{k|k-1}H_x^T(x_{k|k-1})[H_x(x_{k|k-1})P_{k|k-1}H_x^T(x_{k|k-1})+R]^{-1}$
- $x_{k|k}=x_{k|k-1}+K_k(z_k-h(x_{k|k-1}))$
- $P_{k|k}=(I_7-K_kH_x(x_{k|k-1}))P_{k|k-1}$

### (2)AEKF2
>Reference:
>(1)Quaternion_normalization_in_additive_EKF_for_space
>(2)Quaternion normalization in spacecraft attitude determination

- Add brude normalization to quaternion:
set $q_{k|k}=\frac{q_{k|k}}{||q_{k|k}||}$

**Add brute-force normalization is equal to :**
**Initialize step**
- Choose $P_{0|0},x_{0|0}$
- Set $R=\begin{bmatrix}
\sigma^2_b
\end{bmatrix}I_3$
- Set $Q=\begin{bmatrix}
\sigma^2_{\epsilon} \\
\sigma^2_n
\end{bmatrix}I_6$

**Predict step**

- $x_{k|k-1}=x_{k-1|k-1}+\delta tf(x_{k-1|k-1})$ where $w=w^m-\mu$
- $P_{k|k-1}=F_kP_{k-1|k-1}F_k^T+G_k(x_{k-1|k-1})QG_k^T(x_{k-1|k-1})$with star sensor frequency
Among which:
$F_k=\delta tF_x(x_{k-1|k-1})+I_7$

**Update for star sensor:**
- $K_k=P_{k|k-1}H_x^T(x_{k|k-1})[H_x(x_{k|k-1})P_{k|k-1}H_x^T(x_{k|k-1})+R]^{-1}$
- $x_{k|k}=x_{k|k-1}+K_k(z_k-h(x_{k|k-1}))$
- Let $q_{k|k}=\frac{q_{k|k}}{||q_{k|k}||}$
- $P_{k|k}=(I_7-K_kH_x(x_{k|k-1}))P_{k|k-1}(I_7-K_kH_x(x_{k|k-1}))^T+K_kRK_{k}^T$
### (3)AEKF3
**Pseudo-measurement model**

The form of measurement equation is $z_{k+1} = h_{k+1}(x_{k+1} ) + v_{k+1}$

$z_k=q_k^Tq_k+V_k^{nor}= e_1^2+e_2^2+e_3^2+q^2$ and $V_k^{nor}\ \backsim N(0,\sigma_{nor})$

So to linearization:

$\frac{\partial z}{\partial q}=[2e_1,2e_2,2e_3,2q]$
So:

$H_{2x}=[2e_1,2e_2,2e_3,2q,0, 0,0]$

**Initialize step**
- Choose $P_{0|0},x_{0|0},\sigma_{nor}$
- Set $R=\begin{bmatrix}
\sigma^2_b
\end{bmatrix}I_3$
- Set $Q=\begin{bmatrix}
\sigma^2_{\epsilon} \\
\sigma^2_n
\end{bmatrix}I_6$

**Predict step**
- $x_{k|k-1}=x_{k-1|k-1}+\delta tf(x_{k-1|k-1})$ where $w=w^m-\mu$
- $P_{k|k-1}=F_kP_{k-1|k-1}F_k^T+G_k(x_{k-1|k-1})QG_k^T(x_{k-1|k-1})$(with star sensor frequency)
Among which:
$F_k=\delta tF_x(x_{k-1|k-1})+I_7$

**Update for star sensor:**
- $K_k=P_{k|k-1}H_x^T(x_{k|k-1})[H_x(x_{k|k-1})P_{k|k-1}H_x^T(x_{k|k-1})+R]^{-1}$
- $x_{k|k}=x_{k|k-1}+K_k(z_k-h(x_{k|k-1}))$
- $P_{k|k}=(I_7-K_kH_x(x_{k|k-1}))P_{k|k-1}(I_7-K_kH_x(x_{k|k-1}))^T+K_kRK_{k}^T$

**Update for Pseudo-measurement (execute after star sensor update):**
- $K_k=P_{k|k}H_{2x}^T(x_{k|k})[H_2x(x_{k|k})P_{k|k-1}H_2x^T(x_{k|k})+R_2]^{-1}$
- $x_{k|k}=x_{k|k}+K_k(1-h(x_{k|k}))$
- $P_{k|k}=(I_7-K_kH_x(x_{k|k-1}))P_{k|k-1}(I_7-K_kH_x(x_{k|k-1}))^T+K_kR_2K_{k}^T$
#### (4)MEKF
**New space state model**
$x=[\delta e, \delta\mu]^T$
$\delta q=[\delta e,\delta q_0]^T$
Among which :
$\delta q=q^{-1}\hat q$
So: $\frac{d \delta q}{dt}= \frac{dq^{-1}}{dt}*\hat q+\frac{d \hat q}{dt}*q^{-1}$ and $\delta q \approx[\frac{1}{2}\bar \delta e, 1]^T$, as $\delta e\approx 0$

Kinematic equation of $q$:
$\dot q=\frac{1}{2}\Omega q=\frac{1}{2}q*w_q$, where $q$ and $w_q$ are both regarded as $4\times1$ quaternion.

Derivative equation of $\delta q$:
$\dot {\delta q}=\frac{1}{2}(-w*\delta q+\delta q*w)+\frac{1}{2}\epsilon_q*\delta q$ and :

$A=\frac{1}{2}(-w*\delta q+\delta q*w)\\
=\frac{1}{2}\begin{bmatrix}
-2[w\times]\delta e \\
0
\end{bmatrix}$

$G=\frac{1}{2}\epsilon_q*\delta q\\
=\frac{1}{2}\begin{bmatrix}
-[\delta e \times]+\delta qI_3 \\
-\delta e^T
\end{bmatrix}\epsilon$

$\dot {\delta e}=[-w\times]\delta e+\delta \mu+\frac{1}{2}\epsilon$
where:

$\delta \mu=\mu - \hat \mu$
$\dot {\delta \mu}=0$
**New measurement model**
$\delta z=-[\delta e\times]\hat b+\sigma b=-[\hat b\times]\delta e+\sigma b$
where:
$\hat b=A(\hat q)r$
$[\delta e\times]=A(\delta q^{-1})-I_3$
$\sigma b$ are zero-mean, gaussian, white noise
**Adapt delta to quaternion**
$\hat q_{k+1|k+1}=\hat q_{k+1|k}*\delta q^{-1}$
$\hat q_{k+1|k}=\hat q_{k|k}*\delta q^{-1}$
**Initialize step**
- Choose $P_{0|0},x_{0|0},\mu$
- Set $R=\begin{bmatrix}
\sigma^2_b
\end{bmatrix}I_3$
- Set $Q=\begin{bmatrix}
\sigma^2_{\epsilon} \\
\sigma^2_n
\end{bmatrix}I_6$

**Predict step**

$F_k=\begin{bmatrix}
[-w\times], I_3 \\
0 ,0
\end{bmatrix}*\Delta t+I_6$

- $q_{k|k-1}=e^{\frac{1}{2}\Delta t\Omega(w^m-\mu)}$
- $P_{k|k-1}=F_kP_{k-1|k-1}F_k+Q$(with gyroscope frequency)

**Update step**
$\hat b=A(\hat q_{k|k-1})r_k$
$H_k=[-\hat b\times]$

- $K_k=P_{k|k-1}H_x^T(x_{k|k-1})[H_x(x_{k|k-1})P_{k|k-1}H_x^T(x_{k|k-1})+R]^{-1}$
- $x_{k|k}=K_k(b-\hat b))$
- $q_{k|k}=[\frac{1}{2}e_{k|k}, 1]^T$
- $q_{k|k}=\frac{q_{k|k}}{||q_{k|k}||}$
- $q_{k|k}=q_{k|k-1}*\delta q_{k|k}^{-1}$
- $\mu=\mu+\delta \mu_{k|k}$(set it after 200 seconds, avoid too big to fail)
- $P_{k|k}=P_{k|k}=(I_6-K_kH_k(x_{k|k-1}))P_{k|k-1}(I_6-K_kH_k(x_{k|k-1}))^T+K_kRK_{k}^T$

## Part3 - simulation
(Q1)

AEKF1,AEKF2,AEKF3 and MEKF

fighre1.1

mean(errors from aekf1)=9.7103e-03

mean(errors from aekf2)=5.7108e-04

mean(errors from aekf3)=9.3184e-04

mean(errors from Mekf) =3.5506e-04

- According to the results, we can conclude a best appearance of MEKF which has the high speed of decrease, according to the mean of four estimations, the mean of errors in the time range of 500 to 6000 in MEKF also meets the least.

- At the same time, we get the relatively small std value in MEKF as well.
figure1.2

(Q2)

Same thing but with the following initial conditions for the estimation:

- $\hat q_{0/0} = [0:3780; 0:7560; 0:3780;
−0:3780]^T$
- $\hat \mu_{0/0} = 200 [1; 1; 1] \frac{hour}{deg}$ .

Then we get another comparison:


figure2.1
- According to the results showed in the figure, MEKF remains the lowest errors in all filters although it needs the most times to convengence,
This is the standard deviations of the quaternion estimation error, MEKF has a relatively low value of std and has a low fluctuation.
Then we call MEKF as BEKF.

(Q3)

Using BEKF to perform a Monte-Carlo (MC) simulation over 50 runs.
- The estimation error

figure3.1
- The standard deviation of error


figure3.2
As shown in the figure, the orange data is a Monte-Carlo simulation of 50 times and the blue data is the normal MEKF. Apparently, data using Monte-Carlo simulation has lower error,means that the filter is providing more accurate estimates of the system state.

In the figures of $e_1, e_2, e_3$, the value of correlation in Monte-Carlo is lower than the normal one, with the std of $\mu$ reaching the similar results.

(Q4)

When we changes the value of $\hat p_{0/0}$, the results changes as well, here is the comparison between $\hat p_{0/0}=0.001I_7,
\hat p_{0/0}=I_7,
\hat p_{0/0}=100I_7$
figure4.1

But it seems that we can't draw a conclusion with different values of $\hat p_{0/0}$

Then we tested more sets of $\hat p_{0/0}$, which are:
$\hat p_{0/0}=0.00001I_7,
\hat p_{0/0}=0.0001I_7,
\hat p_{0/0}=0.001I_7,
\hat p_{0/0}=I_7,
\hat p_{0/0}=100I_7$

Results shows in this picture:
figure4.2

Results are similar with each other, so we calculated the mean of each result:
fighre4.3

Then we concluded that the changes of $\hat p_{0/0}$ won't influence the appearance of estimation a lot, the errors fluctuated between 3.45e-4 and 3.65e-4 and had at most 3% of increase or decrease.

When we comes to the convariance, it is the same.
fighre4.4

(Q5)
Using BEKF, run three simulations with three different sampling time for the star-tracker: 1second, 5 seconds, and 25 seconds.

figure5.1
This figure shows the blue data (which has the 1s samling time) is the best， then it comes to the 5s (sampling time), then the 25s.

So we can conclueded that the less sampling time is, the smaller error the filter will have.


figure5.2
As the fighre shows, the 1ssampling time data of the correlation is the best.

## Part4 - planning
- solve mekf, give equation and estimation method V
- find out how to set additive noise V
- find out how to generate data
- simulate
- analysis