# Example: # INS Aiding and Error Analysis in 1-D **Local links:** - [[An introduction to state space|StateSpace]] - [[yet another introduction to the Kalman filter|KalmanIntro]] 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|MathSymbols]] 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](http://en.wikipedia.org/wiki/Pink_noise).) The 1-D accelerometer modeled with unknown bias and Gaussian noise has this output [[!teximg code="(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 [[!teximg code="(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 δa = (ã - a), etc. and using the [[continuous-time dynamic equation|StateSpace]], the error dynamics are [[!teximg code="(3)\quad \begin{pmatrix} \rm \delta p \cr \rm \delta v \cr \rm \delta b \end{pmatrix}' = \begin{pmatrix} 0 & 1 & 0 \cr 0 & 0 & 1 \cr 0 & 0 & 0 \end{pmatrix} \begin{pmatrix} \rm \delta p \cr \rm \delta v \cr \rm \delta b \end{pmatrix} + \begin{pmatrix} 0 & 0 \cr 1 & 0 \cr 0 & 1 \end{pmatrix} \begin{pmatrix} \rm \nu_a \cr \rm w_b \end{pmatrix}"]] Assume the noise variables to be stationary and independent [[!teximg code="(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). From the definition [[!teximg code="(5)\quad \bf P \rm [t] \equiv \it E \rm [(\bf x \rm [t] - \bf \bar x \rm [t])^2]"]] The state [[propagation equation|StateSpace]] is [[!teximg code="(6)\quad \bf{x}\rm[t] = \bf\Phi\rm[t] \bf{x}\rm + \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)](#EqNum5) using [(6)](#EqNum6) [[!teximg code="(7)\quad \bf{x}\rm[t] - \bf{\bar x}\rm[t] = \bf\Phi\rm[t] (\bf{x}\rm - \bf{\bar x}\rm) + \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)](#EqNum7) 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)](#EqNum7) and taking the expectation, the terms are independent, so the cross terms are zero [[!teximg code="(8)\quad \bf P \rm[t] = \it E[\bf\Phi\rm[t] (\bf{x}\rm - \bf{\bar x}\rm)^2] + \it E\lbrack\int_0^t \bf\Phi\rm[t-\tau] \bf{G}\rm[\tau] \bf{w}\rm[\tau]\, d\tau"]] In the first term, the (**Φ**) matrix is not stochastic and can be taken outside the expectation yielding [[!teximg code="(9)\quad \it E[\bf\Phi\rm[t] (\bf{x}\rm - \bf{\bar x}\rm)^2] = \bf\Phi\rm[t] \it E[(\bf{x}\rm - \bf{\bar x}\rm)^2] \bf\Phi\rm^T[t] = \bf\Phi\rm[t] \bf P \rm \bf\Phi\rm^T[t]"]] The second term introduces another dummy variable of integration (ς), since the input noise is uncorrelated, define (**Q**) so that [[!teximg code="(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 [[!teximg code="(11)\quad \begin{array}{l} \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\\ \quad = \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 \end{array}"]] Therefore the error covariance at time (t) is [[!teximg code="(12)\quad \bf P \rm[t] = \bf\Phi\rm[t] \bf P \rm \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**) 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)](#EqNum12) is the continuous-time version of the discreet [[formula|KalmanIntro]] worked out on our Kalman Introduction page.) So far we have [[!teximg code="(13)\quad \begin{array}{lll} \bf F \rm \gets \begin{pmatrix} 0&1&0\cr0&0&1\cr0&0&0 \end{pmatrix}, & \bf G \rm \gets \begin{pmatrix} 0&0\cr1&0\cr0&1 \end{pmatrix}, & \bf Q\rm[t] \gets \begin{pmatrix} \rm R_v & 0 \cr 0 & \rm Q_b \end{pmatrix},\\ & & \\ \bf P \rm \gets \begin{pmatrix} \rm P_p&0&0\cr0&\rm P_v&0\cr0&0&\rm P_b\end{pmatrix}, & \bf\Phi\rm[t] \gets \begin{pmatrix} 1 &\rm t &\rm t^2/2 \cr 0 & 1 &\rm t \cr 0 & 0 & 1 \end{pmatrix} \end{array}"]] Where (**Φ**) has been calculated from (**F**) using the [[matrix exponential|StateSpace]] The first term in [(12)](#EqNum12) is the error covariance due to initial uncertainty [[!teximg code="(14)\quad \bf\Phi\rm[t] \bf P \rm \bf\Phi\rm^T[t] = \begin{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 \end{pmatrix}"]] The second term involving the integral in [(12)](#EqNum12) is the error covariance generated by on going noise, call this (**Q**d), the _discreet process noise covariance matrix_ [[!teximg code="(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 = \begin{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 \end{pmatrix}"]] Put both terms together to get the total error covariance [[!teximg code="(16)\quad \bf P\rm[t] = \begin{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 \end{pmatrix}"]] The terms with \{Pp, Pv, Pb\} in them stem from the initial error (**P**). 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 [[!teximg code="(17)\quad \rm \tilde p[t] = p[t] + \nu_p[t]"]] Again, assume white stationary Gaussian noise [[!teximg code="(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 [[!teximg code="(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)](#EqNum3) _Assume_ the feedback matrix is [[!teximg code="(20)\quad \bf L \rm = (0.3, 0.039, 0.002)^T"]] (Note since we choose (**L**) this is not Kalman filtering) Introduce feedback like so [[!teximg code="(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 [[!teximg code="(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](http://www.google.com/search?hl=en&q=%22pole+placement%22&btnG=Google+Search:). 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)](#EqNum16) is not linear in (t). Oh well. Here is the algorithm: (Index (k) counts seconds as the position measurements are made.) [[!teximg code="(23)\quad \begin{array}{lcl} \bf P \rm &\gets& \begin{pmatrix} 0&0&0\cr0&0&0\cr0&0&0 \end{pmatrix}\\ &&\\ \bf P \rm^-[k+1] &\gets& \bf\Phi\rm\bf P \rm[k]\bf\Phi\rm^T+\bf Q\rm_d\\ &&\\ \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{array}"]] In the first step, setting (**P**=*****) is convenient, but otherwise arbitrary. Next (**P**-) is calculated exactly as given in [(12)](#EqNum12) using (Qd) as given in [(15)](#EqNum15). 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 [[!teximg code="(24)\quad \bf P \rm \equiv \it E \rm [(\bf\hat x \rm - \bf{x}\rm)^2]"]] Using the feedback equation [(21)](#EqNum21) [[!teximg code="(25)\quad \begin{array}{lcl} \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{array}"]] But process noise and measurement noise are assumed to be independent, therefore [[!teximg code="(26)\quad \begin{array}{lcl} &&\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{array}"]] Now, armed with an algorithm, a simulation can be run to generate the error graphs. All the matrices in [(23)](#EqNum23) 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) [[!teximg code="(27)\quad \begin{array}{lcl} \bf Q\rm_d \gets \begin{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 \end{pmatrix}, & \bf\Phi\rm \gets \begin{pmatrix} 1&1&1/2\cr0&1&1\cr0&0&1\end{pmatrix},\\ \\ \bf{L}\rm \gets \begin{pmatrix} 0.3\cr0.039\cr0.002 \end{pmatrix}, \qquad \bf{H}\rm \gets \begin{pmatrix} 1\cr0\cr0 \end{pmatrix}, \qquad \bf{R}\rm \gets \begin{pmatrix} 3 \end{pmatrix}, & \\ \\ \bf L\,R\,L\rm^T \gets \begin{pmatrix} 0.27&0.0351&0.0018\cr0.0351&0.004563&0.000234\cr0.0018&0.000234&0.000012 \end{pmatrix}, & \bf{I}\rm - \bf L H\rm \gets \begin{pmatrix} 0.7 & 0. & 0. \cr -0.039 & 1. & 0. \cr -0.002 & 0. & 1. \end{pmatrix} \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)](#FigNum1). (This reproduces Farrel & Barth fig.3.11 p.92) [[!img fig1.png alt="Initial transient response of the filter"]] (Figure 1 is also available as a [[pdf|/Example1D/fig1.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.