Example:
INS Aiding and Error Analysis in 1-D
Local links:
This example is taken from Farrell & Barth, The Global Positioning System & Inertial Navigation, 1999, section 3.4.6 p.91
Using a single one-axis accelerometer the position of a system in one dimension is estimated. The drift that results from integrated accelerometer errors is corrected using a noisy position measurement that is given once per second. The total error of the position estimate is reduced using a pole placement filter. The error is plotted as a function of time.
The remainder of this example uses some specialized notation. For an explanation, see our our mathematical notation page.
1-D Accelerometer with Position Fixes
Real accelerometers have a number of non-ideal characteristics. Two of the most vexing are random noise, and unknown bias. The random noise of an accelerometer output is similar to the noisy output of any measurement system. This random noise is reasonably modeled as Gaussian noise. The unknown bias error arises because the accelerometer output, even at zero acceleration, depends on a variety of factors such as temperature, vibration history, exact power supply voltage, etc. In a real accelerometer it is inevitably found that after correcting for as many variables as is possible, there remains a component of the output that, while slowly varying, is nonetheless unpredictable. This slowly varying unpredictable component is the unknown bias.
(If you want to know more about sources of unknown bias, here is an interesting link about 1/f noise.)
The 1-D accelerometer modeled with unknown bias and Gaussian noise has this output
#!latex (-)
$$\displaystyle (1)\quad
\rm \tilde a[t] = a[t]+b[t]+\nu_a[t]
$$
Where (a) is the acceleration, (b) is the unknown bias, and (νa) is the accelerometer's random output noise
Assume bias (b) is a random walk
#!latex (-)
$$\displaystyle (2)\quad
\rm b[t] = \int_0^t w_b[\tau]\,d\tau
$$
Where (wb) is a white Gaussian noise variable
Defining the error as [[latex($$\delta a = (\tilde a - a)$$)]], etc. and using the continuous-time dynamic equation, the error dynamics are
#!latex (-)
$$\displaystyle (3)\quad
\pmatrix{\rm \delta p \cr \rm \delta v \cr \rm \delta b}' =
\pmatrix{0 & 1 & 0 \cr 0 & 0 & 1 \cr 0 & 0 & 0}\pmatrix{\rm \delta p \cr \rm \delta v \cr \rm \delta b} +
\pmatrix{0 & 0 \cr 1 & 0 \cr 0 & 1}\pmatrix{\rm \nu_a \cr \rm w_b}
$$
Assume the noise variables to be stationary and independent
#!latex (-)
$$\displaystyle (4)\quad
\rm var[\nu_a] = R_v \qquad var[w_b] = Q_b \qquad cov[\nu_a[\tau], w_b[\tau] = 0
$$
Now find the error covariance (P) at time (t).
#!latex (-)
$$\displaystyle (5)\quad
\bf P \rm [t] \equiv \it E \rm [(\bf x \rm [t] - \bf \bar x \rm [t])^2]
$$
The state propagation equation is
#!latex (-)
$$\displaystyle (6)\quad
\bf{x}\rm[t] = \bf\Phi\rm[t] \bf{x}\rm[0] + \int_0^t \bf\Phi\rm[t-\tau] \bf{G}\rm[\tau] \bf{u}\rm[\tau]\, d\tau
$$
Expand the argument of the expectation in (5) using (6)
#!latex (-)
$$\displaystyle (7)\quad
\bf{x}\rm[t] - \bf{\bar x}\rm[t] =
\bf\Phi\rm[t] (\bf{x}\rm[0] - \bf{\bar x}\rm[0]) +
\int_0^t \bf\Phi\rm[t-\tau] \bf{G}\rm[\tau] \bf{w}\rm[\tau]\, d\tau
$$
Where (w) represents the noise components of (u)
Only the noise components of (u) contribute in (7) because the known components effect the state and the average state equally. The noise component doesn't effect the average because zero mean noise has been assumed.
Squaring (7) and taking the expectation, the terms are independent, so the cross terms are zero
#!latex (-)
$$\displaystyle (8)\quad
\bf P \rm[t] =
\it E[\bf\Phi\rm[t] (\bf{x}\rm[0] - \bf{\bar x}\rm[0])^2] +
\it E\lbrack\int_0^t \bf\Phi\rm[t-\tau] \bf{G}\rm[\tau] \bf{w}\rm[\tau]\, d\tau
(\int_0^t \bf\Phi\rm[t-\zeta] \bf{G}\rm[\zeta] \bf{w}\rm[\zeta]\, d\zeta \rbrack)^T]
$$
In the first term, the (Φ) matrix is not stochastic and can be taken outside the expectation yielding
#!latex (-)
$$\displaystyle (9)\quad
\it E[\bf\Phi\rm[t] (\bf{x}\rm[0] - \bf{\bar x}\rm[0])^2] =
\bf\Phi\rm[t] \it E[(\bf{x}\rm[0] - \bf{\bar x}\rm[0])^2] \bf\Phi\rm^T[t] =
\bf\Phi\rm[t] \bf P \rm[0] \bf\Phi\rm^T[t]
$$
The second term introduces another dummy variable of integration (ς), since the input noise is uncorrelated, define (Q) so that
#!latex (-)
$$\displaystyle (10)\quad
\it E[\bf w \rm[\tau] \bf w\rm^T[\zeta]] = \bf Q\rm[\tau]\delta[\tau-\zeta]
$$
Where (δ) is the Dirac delta function
The second term can be rewritten taking the expectation inside the integrals
#!latex (-)
$$\displaystyle (11)\quad
\int_0^t\int_0^t \bf\Phi\rm[t-\tau] \bf{G}\rm[\tau] \it E[\bf{w}\rm[\tau] \bf{w}\rm^T[\zeta]] \bf{G}\rm^T[\zeta]\bf\Phi\rm^T[t-\zeta]\,d\tau\,d\zeta =
\int_0^t \bf\Phi\rm[t-\tau] \bf{G}\rm[\tau] \bf Q\rm[\tau]\bf{G}\rm^T[\tau]\bf\Phi\rm^T[t-\tau]\,d\tau
$$
Therefore the error covariance at time (t) is
#!latex (-)
$$\displaystyle (12)\quad
\bf P \rm[t] = \bf\Phi\rm[t] \bf P \rm[0] \bf\Phi\rm^T[t] +
\int_0^t \bf\Phi\rm[t-\tau] \bf{G} \bf Q\rm[\tau]\bf{G}\rm^T \bf\Phi\rm^T[t-\tau]\,d\tau
$$
For this example, assume (P[0]) is a diagonal matrix {Pp, Pv, Pb}. This assumption reduces the volume of algebra in what follows considerably, and for many practical problems it is a reasonable assumption. Aside from computational complexity, taking (P) as a general matrix presents no additional difficulties.
(The covariance relation in (12) is the continuous-time version of the discreet formula worked out on our Kalman Introduction page.)
#!latex (-)
$$\displaystyle (13)\quad
\begin{array}{lcl}
\bf F \rm \gets \pmatrix{0&1&0\cr0&0&1\cr0&0&0}, & \bf G \rm \gets \pmatrix{0&0\cr1&0\cr0&1}, & \bf Q\rm[t] \gets \pmatrix{\rm R_v & 0 \cr 0 & \rm Q_b}, \\
& & \\
\bf P \rm[0] \gets \pmatrix{\rm P_p&0&0\cr0&\rm P_v&0\cr0&0&\rm P_b}, & \bf\Phi\rm[t] \gets \pmatrix{1 &\rm t &\rm t^2/2 \cr 0 & 1 &\rm t \cr 0 & 0 & 1}
\end{array}
$$
Where (Φ) has been calculated from (F) using the matrix exponential
The first term in (12) is the error covariance due to initial uncertainty
#!latex (-)
$$\displaystyle (14)\quad
\bf\Phi\rm[t] \bf P \rm[0] \bf\Phi\rm^T[t] =
\pmatrix{\rm P_p + P_v t^2 + \frac{P_b t^4}{4} &\rm P_v t + \frac{P_v t^3}{2} &\rm \frac{P_v t^2}{2} \cr
\rm p_v t + \frac{P_v t^3}{2} &\rm P_v + P_v t^2 &\rm P_b t \cr
\rm \frac {P_v t^2}{2} &\rm P_b t &\rm P_b}
$$
The second term involving the integral in (12) is the error covariance generated by on going noise, call this (Qd), the discreet process noise covariance matrix
#!latex (-)
$$\displaystyle (15)\quad
\bf Q\rm d[t] = \int_0^t \bf\Phi\rm[t-\tau] \bf{G} \bf Q\rm[\tau]\bf{G}\rm^T \bf\Phi\rm^T[t-\tau]\,d\tau =
\pmatrix{\rm\frac{Q_b t^5}{20} + \frac{R_v t^3}{3} &\rm \frac{Q_b t^4}{8} + \frac{R_v t^2}{2} &\rm \frac{Q_b t^3}{6} \cr
\rm \frac{Q_b t^4}{8} + \frac{R_v t^2}{2} &\rm \frac{Q_b t^3}{3} + R_v t &\rm \frac{Q_b t^2}{2} \cr
\rm \frac {Q_b t^3}{6} &\rm \frac{Q_b t^2}{2} &\rm Q_b t}
$$
Put both terms together to get the total error covariance
#!latex (-)
$$\displaystyle (16)\quad
\bf P\rm[t] =
\pmatrix{\rm P_p + P_v t^2 + \frac{P_b t^4}{4} + \frac{Q_b t^5}{20} + \frac{R_v t^3}{3} &\rm P_v t + \frac{P_b t^3}{2} + \frac{Q_b t^4}{8} + \frac{R_v t^2}{2} &\rm \frac{P_b t^2}{2} + \frac{Q_b t^3}{6} \cr
\rm P_v t + \frac{P_b t^3}{2} + \frac{Q_b t^4}{8} + \frac{R_v t^2}{2} &\rm P_v + P_b t^2 + \frac{Q_b t^3}{3} + R_v t &\rm P_b t + \frac{Q_b t^2}{2} \cr
\rm \frac{P_b t^2}{2} + \frac {Q_b t^3}{6} &\rm P_b t + \frac{Q_b t^2}{2} &\rm P_b + Q_b t}
$$
The terms with {Pp, Pv, Pb} in them stem from the initial error (P[0]). Those involving {Rv, Qb} account for the subsequent noise.
Notice that the covariance matrix gets larger as (t) increases. That's what is expected using only an accelerometer. The interesting thing is that the error is now quantified. For example the position covariance error grows as the 5th power of time with respect to the bias error variance (Qb). Note this does not mean that the error in position grows as the 5th power of time. The standard deviation is defined as the square-root of the variance, so the position error is growing as (t5/2). Also interesting, the position error with respect to accelerometer noise (Rv) grows as (t3/2).
Now assume noisy position fixes are available at 1 second intervals
#!latex (-)
$$\displaystyle (17)\quad
\rm \tilde p[t] = p[t] + \nu_p[t]
$$
Again, assume white stationary Gaussian noise
#!latex (-)
$$\displaystyle (18)\quad
\rm var[\nu_p] = R_p = 3.0\ m^2
$$
The position fixes can be used to reduce the errors in the state estimate by introducing measurement feedback.
Define the residual output error as the difference between the measured position and the calculated position
#!latex (-)
$$\displaystyle (19)\quad
\rm \delta y[t] \equiv \tilde p[t]-\hat p[t] = \nu_p[t] - \delta p[t]
$$
Where (δp) is as defined in (3)
#!latex (-)
$$\displaystyle (20)\quad
\bf L \rm = (0.3, 0.039, 0.002)^T
$$
(Note since we choose (L) this is not Kalman filtering)
#!latex (-)
$$\displaystyle (21)\quad
\bf\hat x \rm = \bf{x}\rm^- + \bf{L}\rm (\bf\tilde y \rm - \bf{H}\rm \bf{x}\rm^-)
$$
The eigenvalues resulting from this choice are
#!latex (-)
$$\displaystyle (22)\quad
\rm Eigenvalues[\bf\Phi\rm[t] - \bf{L}\rm \cdot \bf{H}\rm] = (0.9 + 0.1\ i, 0.9 - 0.1\ i, 0.9)
$$
Where (i) is the complex unit, and H = (1, 0, 0), that is, we output only position.
Actually, the (L) matrix was chosen specifically to give these eigenvalues, so in essence the feedback matrix was chosen by the method of pole placement.
To plot the growth of state errors through time, a repeated calculation of the covariance matrix (P) is required. It is sufficient to calculate (P-) and (P) at one second intervals. (Just before measurement update, and just after.) The resulting graphs are slightly misleading because the connecting segments between updates will be straight lines, whereas in reality, the segments would be slightly curved since equation (16) is not linear in (t). Oh well.
Here is the algorithm: (Index (k) counts seconds as the position measurements are made.)
#!latex (-)
\begin{eqnarray*}
\bf P \rm[0] &\gets& \pmatrix{0&0&0\cr0&0&0\cr0&0&0}\\
&&\\
(23)\quad \bf P \rm^-[k+1] &\gets& \bf\Phi\rm[1]\bf P \rm[k]\bf\Phi\rm^T[1]+\bf Q\rm_d[1]\\
&&\\
\bf P \rm[k+1] &\gets& (\bf I \rm - \bf L H \rm )\bf P\rm^-[k](\bf I \rm - \bf L H \rm )^T + \bf L\,R\,L\rm^T
\end{eqnarray*}
In the first step, setting (P[0]=*) is convenient, but otherwise arbitrary. Next (P-) is calculated exactly as given in (12) using (Qd) as given in (15). The last step, calculating (P*) uses a formula not seen previously in this example. It represents the change in variance due to the measurement update. Clearly this is an important formula, fortunately, it's not too hard to derive.
By definition the covariance of the state estimate is
#!latex (-)
$$\displaystyle (24)\quad
\bf P \rm \equiv \it E \rm [(\bf\hat x \rm - \bf{x}\rm)^2]
$$
Using the feedback equation (21)
#!latex (-)
\begin{eqnarray*}
(25)\quad
\bf P \rm &=& \it E\rm[(\bf\hat x\rm - \bf{x})^2]
= \it E\rm[(\bf{x}\rm^- + \bf{L}\rm(\bf\tilde y\rm - \bf H x\rm^-)-\bf{x}\rm)^2]\\
&=& \it E\rm[((\bf{I}\rm - \bf L H\rm)(\bf{x}\rm^- - \bf{x}\rm) + \bf L\tilde y\rm - \bf L H x\rm)^2]\\
&=& \it E\rm[((\bf{I}\rm - \bf L H\rm)(\bf{x}\rm^- - \bf{x}\rm) + \bf L(\tilde y\rm - \bf y\rm))^2]\\
\end{eqnarray*}
But process noise and measurement noise are assumed to be independent, therefore
#!latex (-)
\begin{eqnarray*}
(26)\quad
&&\it E\rm[((\bf{I}\rm - \bf L H\rm)(\bf{x}\rm^- - \bf{x}\rm) + \bf L(\tilde y\rm - \bf y\rm))^2]\\
&&= \it E\rm[((\bf{I}\rm - \bf L H\rm)(\bf{x}\rm^- - \bf{x}\rm))^2] +
\it E\rm[(\bf L\rm(\bf\tilde y\rm - \bf y\rm))^2]\\
&&= (\bf{I}\rm - \bf L H\rm)\it E\rm[(\bf{x}\rm^- - \bf{x}\rm)^2](\bf{I}\rm - \bf L H\rm)^T +
\bf{L}\rm \cdot \it E\rm[(\bf\tilde y\rm - \bf y\rm)^2] \bf{L}\rm^T\\
&&= (\bf{I}\rm - \bf L H\rm)\it \bf P\rm^- (\bf{I}\rm - \bf L H\rm)^T + \bf L\,R\,L\rm^T
\end{eqnarray*}
Now, armed with an algorithm, a simulation can be run to generate the error graphs. All the matrices in (23) are constant except the P's. Here are the numerical values of the other matrices involved :
Let (T = 1 second), (Rp = 3.0 m2), (Rv = 2.5x10-3 [m/s2]2), and (Qb = 1.0x10-6 [m/s3]2)
#!latex (-)
$$\displaystyle (27)\quad
\begin{array}{lcl}
\bf Q\rm_d[1] \gets \pmatrix{(8.33383\times10^-4&1.25013\times10^-3&1.66667\times10^-7\cr
1.25013\times10^-3&2.50033\times10^-3&5.\times10^-7\cr
1.66667\times10^-7&5.\times10^-7&1.\times10^-6}, &
\bf\Phi\rm[1] \gets \pmatrix{1&1&1/2\cr0&1&1\cr0&0&1},\\
\\
\bf{L}\rm \gets \pmatrix{0.3\cr0.039\cr0.002}, \qquad
\bf{H}\rm \gets \pmatrix{1\cr0\cr0}, \qquad
\bf{R}\rm \gets \pmatrix{3},
& \\
\\
\bf L\,R\,L\rm^T \gets
\pmatrix{0.27&0.0351&0.0018\cr0.0351&0.004563&0.000234\cr0.0018&0.000234&0.000012},&
\bf{I}\rm - \bf L H\rm \gets
\pmatrix{0.7 & 0. & 0. \cr -0.039 & 1. & 0. \cr -0.002 & 0. & 1.}
\end{array}
$$
Now run the loop 35 times and graph the diagonal elements of (P) as the variance of position, velocity, and bias (Fig.1). (This reproduces Farrel & Barth fig.3.11 p.92)
Upload new attachment "fig1.png"
(Figure 1 is also available as a pdf.)
As pointed out in Farrel & Barth, note that the maximum position variance is about (1 m2), even though the position measurements have a variance of (3 m2). This improvement is possible because the filter averages over several position measurements. The averaging time is reflected in the transient response seen at the beginning of the graphs. The time constant for averaging the bias is larger, which is reasonable.