Assume I have the following gyroscope model in the continuous time:
$
\begin{bmatrix}
\dot{\theta} \\ \dot{\omega_{bias}}
\end{bmatrix} = \begin{bmatrix}0 & 1 \\ 0 & 0\end{bmatrix}
\begin{bmatrix}
\theta \\ \omega_{bias}
\end{bmatrix} + \begin{bmatrix} 1 \\ 0\end{bmatrix} \omega_t + \begin{bmatrix} w_{wn} \\ w_{bias}\end{bmatrix}
$
Where:
$\omega_t $ – true angular velocity.
$w_{wn} = \sigma_{wn}*w_{n}, \{ \sigma_{wn} = \sigma_{n}*\frac{1}{\sqrt{dt}}, w_{n} = N(0,1) \}$
white noise with given $\sigma_{n}$.
$w_{bias} = \sigma_{bias}*w_n, \{ \sigma_{bias} = \sigma_{b}*\sqrt{dt}, w_{n} = N(0,1) \}$
gyroscope bias with given $\sigma_{b}$.
My goal is to derive Kalman filter for the state vector:
$ x = \begin{bmatrix} \theta \\ \omega_{bias} \end{bmatrix}$
In the discrete time prediction equation is as follows:
$
\begin{bmatrix}
\theta \\ \omega_{bias}
\end{bmatrix}_{k+1} = \begin{bmatrix}1 & dt \\ 0 & 1\end{bmatrix}
\begin{bmatrix}
\theta \\ \omega_{bias}
\end{bmatrix}_{k} + \begin{bmatrix} dt \\ 0\end{bmatrix} u_k
$
The problem here is since $u_k$ comes from the gyroscope, it already contains the bias part of the angular velocity, which I don't know exactly. But I have it's estimation on the previous step, so I can remove it like this:
$
\begin{bmatrix}
\theta \\ \omega_{bias}
\end{bmatrix}_{k+1} = \begin{bmatrix}1 & dt \\ 0 & 1\end{bmatrix}
\begin{bmatrix}
\theta \\ \omega_{bias}
\end{bmatrix}_{k} + \begin{bmatrix} dt \\ 0\end{bmatrix} (u_k – \omega_{bias})
$
However it is awkward and it becomes unclear how to compute process noise covariance matrix $Q$ in this case.
Can somebody provide an explanation how to work out bias estimation properly?
Best Answer
A long explanation to a simple answer, but hopefully, this helps in your understanding.
The process model is a vector-values function of the states $x_k$ (i.e., $x_k = \theta_k$), control inputs $u_k$ (e.g., $u_k = \omega_k$), and the process noise $w_k$. $$x_{k+1} = f(x_k, u_k, w_k)$$
Any parameter in the process model that is not $x_k$, $u_k$, or $w_k$ is a constant, which is known. If not, you have something wrong. I suggest either writing out the individual equations for the process model, then deriving the necessary matrices. Often, the noise is multiplicative instead of additive, and in such cases, following the described procedure make deriving the matrices easier.
I believe you have the following equations for the discrete time process model (correct me if I am wrong): $$ \begin{align} \theta_{k+1} &= \theta_k + \tau(\omega_k + w_k - b_t) \\ b_{t+1} &= b_t \end{align} $$ where $\tau$ is the time step (i.e., $\tau = dt$), $b_t$ is the gyro bias, $w_k = \omega_{wn} + \omega_{bias}$ where $\omega_{wn} \sim \mathcal{N}(0, Q)$ where $Q = \sigma_{wn}^2$ is the covariance matrix (except in this example $Q$ is a scalar instead of a matrix). Note, I use $b_t$ as the bias at time step $t$ instead of $\omega_{bias}$ because $\omega_{bias}$ is the true bias, which is unknown. Here, I assume the bias is constant, but the bias may also be time varying.
Now, I assume you need $Q$ for implementing a Kalman filter (KF), and I think the previous discussion answer your question. However, you do not have additive noise (assuming $\tau$ is not constant which is usually the case in practice), which you seem to understand (even if by accident) because I see you using $\tau$ in the computation of your $Q$ matrix. However, you would be required to change $Q$ at each iteration of the KF, which is okay (and done often in practice), but I this makes things more confusing (unless $Q$ is actually time varying rather than the time step).
Now, in the KF, we have the following Jacobians associated with the process model $F = \frac{\partial f}{\partial x}(x_k, u_k, 0)$ and $L = \frac{\partial f}{\partial w}(x_k, u_k, 0)$. Notice, $w_k = 0$ in the computation of the Jacobians. Now, see the difference between equations for additive noise and multiplicative noise for the predict step of the error covariance.
Additive Noise: $$ P_{k+1} = F_k P_k F_k^T + Q_k $$
Multiplicative Noise: $$ P_{k+1} = F_k P_k F_k^T + L_k Q_k L_k^T $$
If you work out $L_k$ given you process model, then you have the following $$ L_k = \begin{bmatrix} \frac{\partial f_1}{\partial w} \\ \frac{\partial f_2}{\partial w} \end{bmatrix} = \begin{bmatrix} \tau \\ 0 \end{bmatrix} $$ So, $$ L_k Q_k L_k = \begin{bmatrix} \tau^2 \sigma^2_{wn} & 0 \\ 0 & 0 \end{bmatrix} $$