One Dimensional INS Design Example

∗
Jay A. Farrell
March 16, 2021

Abstract but which cannot or need not be implemented in software so-

.
lutions. The symbol ‘=’ will be used to indicate computations
Many students try to jump into very challenging Kalman fil- that will be implemented in the software. This distinction is
ter designs without ever developing a strong understand of a meant to help people new to the field to distinguish between
simple design. This often leads to frustration. A curse of the aided INS implementation and the analysis of that implemen-
Kalman Filter is that it is about five equation, see the Ap- tation.
pendix, which seem very easy to understand and implement; Boldface symbols represent matrices or vectors.
however, those five equations are built on a set of models,
parameters, and theoretical ideas, which if not accurate and
understood will lead to unsatisfactory implementations. 2 Problem Statement
This document presents the basic ideas of aided inertial
navigation using a simplified one-dimensional example using A sensor is constrained to move in a single direction while
IMU data. These basic ideas include the system kinematic measuring its acceleration in that direction. The system does
equations, the INS implementation, the error model, quan- not rotate. The task is to estimate the position and velocity
tification of the model parameters, estimation of the model of the sensor in that one direction as a function of time t. At
errors by a Kalman filter, and correction of the INS state. certain instants of time Tk for k = 1, 2, 3, . . . , measurements
Special attention is paid to the development of the state of the sensor position are available.
transition model for the Kalman filter. The simplified one- The sensor is an Inertial Measurement Unit (IMU). The
dimensional model removes various complicating factors that IMU provides acceleration measurements ũ(ti ) for i = 0, 1, 2, . . .
1
arise in a full INS (e.g., nonlinearity and attitude), to enable where ti = iτ and τ = fs , The IMU was selected so that the
the reader to focus on the basic INS and Kalman filtering sampling rate fs is at lease twice the bandwidth of the motion
ideas. Understanding the basics is critical before moving to of the sensor. It will typically be the case that (Tk −Tk−1 ) >>
more complicated scenarios. τ , so that there are many IMU measurements between posi-
The article is intended to come along with at least one tion measurements.
IMU data set. The intent is as follows:
• Readers complete the design themselves and implement 3 INS Approach and Background
it in Matlab or Python testing their implementation us-
ing the provided data. This section develops the time differential equations for the
state vector of the Inertial Navigation System (INS).
• The solution is provided so that readers can get help
when they get stuck. Please try to solve the problem 3.1 Kinematic Model
on your own. You miss the valuable learning and imple-
mentation experience if you simply read the solution. The position p(t), velocity v(t) , and acceleration a(t) are
related to each other by the kinematic model
If you are a UCR student, please contact me with questions.
I am providing this document and data as an educational ṗ(t) = v(t) (1)
service. v̇(t) = a(t). (2)
If you find errors in the document or implementation,
please do let me know. It is important to understand the difference between kine-
matic and dynamic models. The kinematic model is pristine,
without any sources of error and no parameters that might
1 Notation be inaccurate.
A dynamic model would account for how the acceleration
In this document, the equality symbol ‘=’ will be used in its
a was affected by position, velocity, and external inputs. A
normal sense. It is used in models that are used for analysis,
dynamic model is necessary for control system design and
∗† Professor at the Dept. of Electrical and Computer Engineering,
analysis. A kinematic model is used for Inertial Navigation
UC Riverside. {farrell}@ece.ucr.edu. System (INS) design and analysis.

1
3.1.1 IMU Model yk = y(kT ), pk = p(kT ), and ηk = η(kT ). In matrix form,
eqn. (7) can be written as
In this article, the accelerometer measurement ũ ∈ < will be
modeled as y(kT ) = H xk + ηk (8)
ũ(t) = a(t) − z(t) (3)
where a(t) is the acceleration and z(t) represents additive with H = [1, 0, 0].
measurement errors. One challenge in INS implementation
is characterizing the IMU errors in a manner that is compat- 3.3 INS Implementation
ible with the solution approach (i.e., as a state space model
Let p̂(t), v̂(t), and b̂(t) represent the INS estimate of the po-
with finite dimension).
sition, velocity, and IMU bias at time t.
For the present example, assume that
Based on eqns. (1-2) and taking the expected value of
z(t) = b(t) + n(t) (4) both sides of eqn. (5), the continuous-time INS model is
ḃ(t) = ω(t). (5) ˙
p̂(t) = v̂(t) (9)
˙
v̂(t) = â(t) (10)
This is one of the simplest IMU error models. In this model,
n(t) is white Gausian measurement noise with power spectral ˙
b̂(t) = 0 (11)
density Qn = σn2 , ω(t) is white Gaussian measurement noise
with power spectral density Qω = σω2 . Such models are dis- where â(t) = ũ(t) + b̂(t). Because the IMU measurements are
cussed in Section 4.4.2-4.6 in [1]. The symbol n(t) is referred taken at the discrete-time intervals ti = iτ , these equations
to as the velocity random walk error component. The symbol are implemented in discrete-time, for example:
b(t) is often referred to as the sensor bias, it represents time
.
correlated measurement errors. p̂i+1 = p̂i + v̂i τ + 0.5âi τ 2 (12)
Do Problem 5.1 described on p. 5. .
v̂i+1 = v̂i + âi τ (13)
.
b̂i+1 = b̂i (14)
3.1.2 System State Space Model
.
From eqns. (1-2) and (5), the continuous-time state-space with âi = ũi + b̂i .>The discrete-time INS state estimate vector
model is is x̂i = [p̂i , v̂i , b̂i ] . Eqns. (12-14) are computed at the IMU
      rate fs .
0 1 0 0 0 In matrix state space form eqns. (12-14) become
ẋ(t) =  0 0 0  x(t) +  1  a(t) +  0  ω(t) (6)
0 0 0 0 1 x̂i+1 = Φi x̂i + Gi ũi (15)

1 τ 0.5 τ 2 0.5 τ 2
   
>
where the state vector is x(t) = [p(t), v(t), b(t)] .
Note the following points: with Φ i =  0 1 τ , and Gi =  τ .
0 0 1 0
• The state vector includes the kinematic state [p(t), v(t)]
and the sensor calibration factor b(t). These are the Remark 3.1 The state space model in eqn. (15) is not ap-
quantities that will be estimated. By estimating IMU propriate for Kalman filter design, because it does not match
sensor calibration factors, such as b(t), the INS accuracy eqns. (52). Eqn. (15) depends on the signal a(t), through
will be enhanced. ũ. The signal a is not a known deterministic signal, nor is it
a signal that can be modeled stochastically with known Gaus-
• The symbol n(t) does not appear in eqn. (6), because sian distribution. Therefore, the Kalman filter cannot yet be
the sensor measurement error z(t) has no effect on the applied, additional manipulations are required. 4
time evolution of the actual state vector x(t).
• Nothing in the above corresponds to an INS computa- Remark 3.2 Eqns. (12-14) are computed at the IMU rate
tion. It is only system modeling. fs . The subscript ‘i’ is being used to denote the INS state
variables accumulated at the IMU rate. Assuming that Tτ = L
3.2 Aiding Sensor Model is an integer, there will be L INS iterations between any two
consecutive aiding measurements: T = L τ . Starting at time
In this example, the aiding sensor measures position at T = 20
zero, the INS updates occur at ti = i τ for i = 0, 1, . . . , L − 1.
second intervals. The sensor measurement is modeled as
The INS uses ũL−1 = ũ((L − 1)τ ) to compute
y(kT ) = p(kT ) + η(kT ) (7)
x̂i i=L
= x̂(Lτ ) = x̂(T ) = x̂k k=1
.
where η(t) is white Gaussian noise with variance ση2 .
To sim-
The discussion of the Kalman filter will use the subscript k to
plify notation, the document will use the following symbols:
denote signals at the time of the aiding measurements: xk =

2
  2 
x(kT ), yk = y(kT ), etc. Symbols such as x3 will not be and Q = . σni 0
d . For more complex problems, meth-
used, as it is unclear whether it means x(3τ ) or x(3T ). The 0 σω2 i
subscript ‘i’ will always refer to the INS time step τ . The ods to compute Φ and Qd from F, G, and Q are discussed in
subscript ‘k’ will always refer to the Kalman Filter time step Sect. 4.7 of [1].
T. 4 The aiding sensor prediction error, called the residual mea-
surement, is defined to be
Once p̂(k T ) becomes available at t = kT , using the ex- rk = yk − ŷk . (26)
pected value of eqn. (7), the INS can predict the sensor mea-
surement as Substituting in eqns. (8) and (17), the residual can be mod-
ŷ(kT ) = p̂(kT ). (16) eled as
rk = Hδxk + ηk . (27)
In matrix form, eqn. (16) can be written as
Note that eqn. (25) is not useful directly and cannot be
ŷ(kT ) = H x̂k (17) integrated, since neither δxi nor ωi are known. Nonetheless,
the analysis of the error growth that results from this equation
with H = [1, 0, 0]. is key to the design of the optimal state estimator, as eqn. (25)
Do Problem 5.2 described on p. 5. does match the form of eqn. (52) as required for the design
of the Kalman filter.
3.4 Error State Model One last issue is that eqn. (25) accounts for error growth
over one IMU time period, while eqn. (27) occurs after N = Tτ
This section develops a model for the time evolution of the IMU time cycles. The error accumulation over these N IMU
error in an INS implementation. The model is in state space cycles is discussed in the next section.
form with stochastic inputs having known statistics, as is ap- Do Problem 5.3 described on p. 5.
propriate for development of an optimal state estimator (i.e.,
Kalman filter).
3.5 Covariance Propagation
Assuming that the IMU is sampled fast enough that the
acceleration can be considered constant over each sampling The uncertainty in the state estimate x̂i is characterized by
period, the discrete-time version of eqns. (1-2) and (5) is the covariance matrix of the error state:

pi+1 = pi + vi τ + 0.5ai τ 2 (18) cov(x̂i ) = cov(δxi ) = Pi . (28)

vi+1 = vi + ai τ (19) Using eqn. (25), Section 4.65 in [1] shows that given an initial
bi+1 = bi + ωi (20) value P0 the error covariance matrix is propagated through
time at the IMU rate as
where ωi is a white Gaussian discrete-time noise process with .
Pi+1 = Φi Pi Φ> >
i + Γi Qd Γi . (29)
second-order statistics equivalent to those of ω(t) at the IMU
sampling instants. Eqn. (29) needs to be propagated T fs times (i.e., at each
The discrete-time error state vector is IMU sampling instant) between aiding measurements (occur-
ing with period T ).
δxi = xi − x̂i . (21) Dropping the time subscript for this paragraph, to simplify
the notation, the structure of P is
Differencing eqns. (18-20) with eqns. (12-14), the discrete-
σp2
 
time difference equations for the error state simplify to ρpv σp σv ρpb σp σb
P =  ρpv σp σv σv2 ρvb σv σb  (30)
2 2
δpi+1 = δpi + δvi τ + 0.5δbi τ + 0.5ni τ (22) ρpb σp σb ρvb σv σb σb2
δvi+1 = δvi + δbi τ + ni τ (23)
where σ∗2 represents the covariance of variable * and ρab is the
δbi+1 = δbi + ωi (24) cross-correlation between variable a and b. Therefore, once P
is computed at any time, the error covariance of any element
where ni is the discrete-time Guassian white noise process
of the state vector can be computed. The only assumption
equivalent to n(t). These equations can be written in the
for this to be valid is that the IMU error model of eqns. (3-5)
matrix state-space form
and its parameters Qn and Qω must be accurate.
δx = Φ δx + Γ ω (25) Do Problem 5.4 described on p. 5.
i+1 i i i i

where ωi = [ni , ωi ]> is Gaussian white noise with covariance 3.6 INS Implementation Summary
Qd and
This section summarizes the INS implementation equations
1 τ 0.5τ 2 0.5τ 2 0 over the time interval t ∈ [tk−1 , tk ], assuming the initial con-
   
.  . dition xk−1 ∼ N (x̂k−1 , Pk−1 ). At the IMU rate fs , as each
Φi = 0 1 τ  , Γi =  τ 0 ,
0 0 1 0 1 IMU measurement ũi arrives:

3
INS state propagation: The INS computes eqns. (12-13). Due to eqn. (31), the prior has distribution δx−
k = 0; there-
fore, eqn. (38) would reduce to
INS state error covariance propagation: The INS com-
putes the error state covariance matrix using eqn. (29). δx+
k = Kk rk . (39)
After incorporating all fs IMU measurements in [tk−1 , tk ), the Using eqn. (21), the corrected INS state estimate is
INS has effectively implemented the Kalman filter’s task, as
stated in eqns. (54-55), of propagating the state estimate and x+ k = x− k + δxk
+
(40)
its error covariance matrix between the times of the last and + −
xk = xk + Kk rk , (41)
the current aiding measurements.
This computed INS state vector x̂ is communicated at which matches eqn. (37). After correcting the INS state as
any sub-multiple of fs to whichever other systems might be in eqn. (37), then x+ = Ehxk |yk , . . . , y1 i and δx+ = 0.
k k
relying on it: control, planning, etc.
4.3 KF Residual Sequence
4 Kalman Filter Implementation If the INS and Kalman filter are properly implemented with
correct parameter settings, then the residual should be zero
As stated in Section 3.6, the INS has already implemented
mean, white, and have covariance Sk . The designer should
the time propagation step of the Kalman filter; therefore, this
always check the covariance sequence to ensure that this is
section discusses the Kalman filter measurement update. For
the case. If it is not, then debugging is required.
the measurement update, the Kalman filter prior is the result
Designing of the Kalman filter should not involve any ‘tun-
of the last step of the INS time propagation over the last
ing’ of the parameters in Qd . These parameters relate to the
interval (see Remark 3.2):
performance of the IMU and should be determined from the
x̂k = Ehxk |yk−1 , . . . , y1 i = x̂i and Pk = Pi with i = T fs . specification parameters of the IMU that are supplied by the
− −

(31) manufacturer.
− −
Therefore, x̂k ∼ N (xi , Pk ) i=T f . Eqn. (17) is used to com- After reading the material in this section, complete Prob-
s
pute the prior output: lem 5.5 on page 6.

.
ŷk− = H x̂−
k. (32)

4.1 KF Implementation
The measurement residual is computed as
.
rk = yk − ŷk− . (33)

The covariance of the residual is

.
Sk = HP− > 2
k H + ση . (34)

The Kalman measurement update is

.
Kk = P− >
k H (Sk )
−1
(35)
.
P+
k = P− −
k − Kk (H Pk ) (36)
.
x+
k = x−
k + Kk rk . (37)
−
The posterior state x+ k and covariance Pk are now available
as initial conditions for the next set of INS computations as
summarized in Section 3.6.

4.2 KF Clarification
It is important to clarify an issue that is not obvious in the
presentation of Section 4.1. The Kalman filter was actually
designed for the error model. Therefore, eqn. (37) should
actually be

δx+
k = δx−
k + Kk rk . (38)

4
5 Reader Exercises 3. Assume that the IMU may have a scale factor error. In
this case, eqn. (3) would be modified to:
Test your understanding of the above materials by completing
the following. ũ(t) = (1 − s)a(t) − z(t) (42)

Problem 5.1 Unzip the data files and see the README.txt. where s is a small constant with random initial value.
for a data description.
Analyze the basic data to ensure that it looks as expected. (a) Show that δa defined as δa = a − â has the lin-
Note the following: earized model:
â 1 1
• Find a time segment during which the IMU is station- δa = δs + δb + η. (43)
ary. Look closely at the measured acceleration during 1 − ŝ 1 − ŝ 1 − ŝ
this time. You are checking reasonableness. A real IMU (b) Revise the error model to account for the scale fac-
would have a spec sheet and you would analyze the mea- tor error δs as an additional state variable.
sured IMU data relative to that specification.
• The time vector in each data structure is the received
time at the CPU, not the measurement time at the Problem 5.4 In this problem treat the time interval t ∈
sensor. Analyze the delta time between measurements. [1, 20]s as 19 different data sets that are each two seconds
Does it meet the specification? long.
The IMU is at rest at the origin during this entire time
• The sensor data can be thought of as coming from 20 interval; therefore, any deviation of the position and velocity
second long experiments starting at t = 20. During each portion of the state estimate from 0 is INS state error. In
20 second interval, the IMU has some 1-D motion, but this problem, you will generate 19 instances of the INS state
it is stationary at a known position at estimation error and compare that with the computed error
state covariance.
tk = k20.
1. For each two second interval t ∈ [`, ` + 2) for ` =
Check the data to ensure that this is true. 0, . . . , 18, complete the following steps:

(a) Average the N = fs samples of acceleration data

Problem 5.2 Implement a pure INS (no aiding). for t ∈ [`, ` + 1) to estimate the bias b̂` and its
covariance [P]33 = σb2 . Note that for the values of
1. Starting at t = 0 with position, velocity, and bias equal
σb2 = var(n) should be essentially the same for each
to zero (i.e., x̂(0) = [0, 0, 0]> ), use eqns. (12-14) to N
averaging interval.
integrate the INS state over the interval for which the
IMU data is available. (b) At t = ` + 1, define the initial state estimate to be
x̂ = [0, 0, b̂` ]> , with initial covariance matrix
2. Use the data for t ∈ [1, 20]s to estimate the bias:  
0 0 0
20
−1
Z
b̄ = ũ(t)dt. P= 0 0 0 .
20 0 0 0 σb2

3. Starting at t = 20 with position and velocity equal to Use the IMU data for t ∈ [` + 1, ` + 2) and eqns.
zero, and the bias equal to the value estimated in Step (12-14) to integrate the state estimate over this one
2 (i.e., x̂(20) = [0, 0, b̄]> ); use equations (12-14) to inte- second interval. Call this one second long trajec-
grate the INS state over the interval for which the IMU tory
data is available.
Xl = [x(`+1), x(`+1+τ ), x(`+1+2τ ), . . . , x(`+2)].
4. Estimate the initial condition for the bias by any other
method you wish and repeat the INS integration to com- For all 19 values of `, plot the position and velocity
pare performance. states from X` versus the integration time,

t̄ = t − (` + 1) (44)
Problem 5.3 = [0, 1, 2, . . . , N ]τ. (45)

1. Analyze the difference between eqns. (12-14) and (18- This common time axis facilitates the subsequent
20) to derive eqns. (22-24). analysis.
2. Use eqns (8), (17), and (26), to derive eqn. (27).

5
 2. Use eqn. (29) to propagate the error state covariance
matrix Pi as a function of the integration time t̄i = iτ .
m/s2
Assume that σn = 1 × 10−3 m/s
√ and σw = 1 × 10−5 √
s s
Note that this sequence will be essentially the same for
each value of `, so only needs to be computed once.
From Pi extract σp (i) and σv (i) using eqn. (30). Plot
±σp (i) on the position graph from Step 2 and ±σv (i)
on the velocity graph from Step 2.
3. If the values of σn2 and σω2 are defined accurately, then
about 68% of trajectory points will be within the ±1σ
envelope. Does this approximately hold for your imple-
mentation?
4. For this simple problem, the error covariance can be
found in closed form as
σn2 3 σb2 4 σω2 5
Pp (t̄) = t̄ + t̄ + t̄ (46)
3 4 20
σ2
Pv (t̄) = σn2 t̄ + σb2 t̄2 + ω t̄3 . (47)
3
Check that all the units in each term of these equations
work out correctly. Each term can be plotted separately
to determine which error source is dominant over a one
second window.

Problem 5.5 Implement the Kalman filter to correct the

INS state estimate at the times t = k 20. Analyze the resid-
uals relative to their expected standard deviation. For this
exercise let ση = 0.01m.

6
6 Solutions
This solution works with simulated data. For this data set,
the known position at the stationary instants is p(Tk ) = 0.

6.1 Solution for Problem 5.1

This section analyzes the raw accelerometer data to ensure
reasonableness of the data and checks any assumptions.

Figure 3: Measured acceleration ũ(ti ) for ti ∈ [1, 20].

readings are biased below zero by about 0.01m/s/s. Second,

the standard deviation is 0.001 m/s/s. Third, other than the
bias, there are no obvious correlations in the data. The cor-
relation sequence can be plotted in Matlab using the ‘xcorr’
function.

Figure 1: Analysis of the IMU delta-time δt between samples.

Fig. 1 shows a histogram and time series for the delta Figure 4: Measured acceleration ũ(ti ) for ti ∈ [38, 122].
time between IMU samples. Because this is simulation data
it is perfect with δt = τ = f1s for all samples. For actual
Fig. 4 shows the measured acceleration over four intervals
data this is not typically the case. For actual data there will
of motion. Each interval of motion involves oscillation and
actually be two different δt values: the value reported by the
returns to its original position with zero velocity at Tk = k 20.
IMU and the value reported by the CPU. The CPU includes
The FFT of this signal is shown in Fig. 5. The majority of the
communication and processing latency. The INS integration
signal power is below 10 Hz, which is viewed as the bandwidth
process should use the δt reported by the IMU, not the CPU.
of the system; therefore, the assumption that the Nyquist
frequency fN = fs /2 = 62.5Hz should be significantly higher
than the system bandwidth is verified.

Figure 2: Measured acceleration ũ(ti ) for ti ∈ [1, 600].

Fig. 2 plots the measured acceleration versus time. The Figure 5: FFT(ũ(ti )) for ti ∈ [38, 122].
maximum acceleration is less than 1 g = 9.8m/s/s. The an-
alyst must ensure that the IMU measurement range is (more Since there are no problems with the data set or the as-
than) sufficient for the expected operating scenarios. Several sumptions, the analysis and implementation proceed to the
periods of oscillatory motion are obvious. At this scale, it is next step.
challenging to observe anything more specific. In the real-time implementation, care must be taken at
Fig. 3 shows the measured acceleration for the first twenty start-up to ensure that the buffer is cleared and that the sen-
seconds of data. During this time interval, the sensor is sta- sor has time to turn on and start sending data.
tionary, allowing a few interesting observations. First, the

7
6.2 Solution for Problem 5.2

Figure 6: INS computed position without aiding for three

different initial conditions for the bias.

Fig. 6 displays the INS computed position for three differ-

ent estimates of the bias. The green curve initializes the bias
b̂20 = 0. Over the 600 second interval of integration, the posi-
tion error grows to -1428m. The red curve initializes the bias
to the average over the first 20 seconds: b̂20 = b̄. Over the
600 second interval of integration, the position error grows to
265m, which is better, but not very great. The blue curve ini-
tializes the bias to a value (explained in the next paragraph)
that causes the final position to be zero. Even though the Figure 7: INS trajectory for initial bias estimate -0.0135
blue curve has a correct final value, its maximum position m/s/s.
error near t = 380s is approximately 39 m.
Consider the green curve again, its near parabolic shape Because this experiment was designed with long periods
can be used to estimate an effective value of the bias over the when the sensor is stationary, to maintain accuracy the inter-
full time interval. Using the model that the position error due ested reader could compute bias estimates periodically dur-
to integration of a bias over an interval T should be p̂(T ) = ing those stationary time intervals. Such methods tuned to
0.5bT 2 yields a specific experiment are fun to play with, but are ad-hoc
b̂ = 2p/T 2 = 2(1428.5)/(580)2 m/s/s. (48) and non-optimal. Subsequently, we will investigate general
purpose optimal methods.
This is the bias value used to compute the blue curve. Note The purpose of these graphs are to illustrate a few key
that there is no constant bias value that is correct. The bias points:
is time-varying and must be estimated in real-time.
• The INS is an integrative process. The information
Fig. 7 shows portions of the position and velocity trajec-
being integrated include errors in initial condition and
tories computed by the INS using the bias estimate from eqn.
from the IMU; therefore error accumulates in the kine-
(48). The top subplot shows the INS computed velocity at all
matic state integrated by the IMU. These errors accu-
times. This plot is included to show that the motion of the
mulate slowly and can be accurately modeled.
IMU (i.e., the oscillations) is superimposed on another signal.
To visualize that signal, the middle subplot shows the IMU • Because the initial condition and IMU errors are ran-
computed velocity at the times Tk = k 20s, which are those dom, the error model will be built using stochastic mod-
times at which we know from the design of the experiment els.
that the IMU is in fact stationary. These are the velocity
errors. Similarly, the third subplot shows the position errors, • Estimation of the IMU calibration factors (e.g., the bias)
which are the values of the INS computed position at Tk , is important.
when we know that the IMU is sitting at the origin. Com- • The bias is time varying. Even if accurately estimated
paring these two error plots, you should be able to see that over a time interval, its value will change versus time
the position error is the integral of the velocity error and the resulting in velocity and position errors; therefore, its
point at which the position error has maximum magnitude is value must be estimated persistently.
the point at which the velocity error is zero.

8
6.3 Solution for Problem 5.3
For Part 3, assume that ṡ(t) = 0 and s(0) ∼ N (0, Ps (0)).
The estimator will provide ŝ = Ehs(t)|y(τ ) ∀τ < ti and b̂ =
Ehb(t)|y(τ ) ∀τ < ti. The scale factor error is defined as
δs(t) = s(t) − ŝ(t). Taking the expected value of both sides
of eqn. (42) yields

Ehũ(t)|y(τ ) ∀τ < ti = (1 − ŝ(t))a(t) − b̂. (49)

Based on eqn. (49), given a measurement ũ(t), the accelera-

tion would be computed as
1  
â(t) = ũ(t) + b̂ . (50)
1 − ŝ(t)
To find the error model, substitute in eqns. (42) and (4), then
simplify
1  
â(t) = (1 − s)a(t) − z(t) + b̂
1 − ŝ(t)
1  
â(t) = (1 − s)a(t) − (b(t) + n(t)) + b̂(t)
1 − ŝ(t)
1−s 1   Figure 8: Nineteen examples of INS errors each integrated
â(t) = a(t) + b̂(t) − b(t) − n(t) . over one second. Each example is plotted in its own color,
1 − ŝ(t) 1 − ŝ(t)
starting with position and velocity equal to zero and bias
The following will utilize the definitions: δb(t) = b(t) − b̂(t), equal to the average over the previous one second.
and δa(t) = a(t) − â(t).
1 − (δs(t) + ŝ(t)) 1
â(t) = a(t) − (δb(t) + n(t))
1 − ŝ(t) 1 − ŝ(t)
a(t) 1
â(t) = a(t) − δs(t) − (δb(t) + n(t))
1 − ŝ(t) 1 − ŝ(t)
â(t) + δa(t) 1
â(t) = a(t) − δs(t) − (δb(t) + n(t))
1 − ŝ(t) 1 − ŝ(t)
which yields
â(t) 1 1
δa(t) = δs(t) + δb(t) + η(t). (51)
1 − ŝ(t) 1 − ŝ(t) 1 − ŝ(t)
after dropping the second order product term with δa(t)δs(t).
Note that the coefficients in this equation can be found
directly as the partial derivative of â with respect to the cal-
ibration parameter. For example, ∂∂b̂ â = 1−ŝ
1
.

6.4 Solution for Problem 5.4

Fig. 8 shows the 19 error simulations, each starting with
position and velocity at zero and bias initialized to the average
sensor reading over the last one second. The 19 values of the
bias are shown in the third figure from the top. The value
of σb was approximately 1.0 × 10−4 m/s/s. The position and
Figure 9: The same 19 INS error examples as in Fig.8, plotted
velocity error trajectories are shown in the top two plots.
with a common time axis. Each subplot is overlaid with a
Fig. 9 shows the same 19 position and velocity error tra-
wide black graph of ±σ where sigma is extracted from the
jectories plotted versus integration time (i.e., a common axis).
error covariance matrix P computed by eqn. (29).
The subplots of this figure also show a wide black curve that
shows ±σp and ±σv as extracted from the Pk computed using
eqn. (29) and extracted using the structure of P as defined
in eqn. (30).

9
Figure 10: Kalman filter residual
√ rk and the envelope of its
residual ±σrk where σrk = Sk .

Figure 11: Kalman filter corrected INS estimate of the state

6.5 Solution for Problem 5.5 vector (blue). State estimate plus and minus the state error
Fig. 10 plots the Kalman filter residuals versus time as dots, standard deviation (black).
along with the computed standard deviation of the residuals
as a solid black curve. The residual properties (zero mean,
uncorrelated in time, standard deviation about right) look
acceptable. In steady-state, the position error after 20 seconds
of uncorrected motion is expected to be about 0.13m.
Figs 11 and 12 show magnified views of the INS state dur-
ing two time periods. The blue curves represent the Kalman
filter corrected INS estimate of the state vector. The black
curves are that state estimate plus and minus the state error
standard deviation as extracted from the diagonal elements
of P as explained relative to eqn. (30).
Note that:
• The actual motion of the IMU is clearly evident in the
position and velocity plots.
• Error does accumulate during each 20 second interval of
INS integration between Kalman filter corrections.
• The high frequency IMU measurement noise, see Fig.
4, is effectively removed by the INS integration process.
Integration is low pass in nature.

• The INS error accumulation is effectively removed by

the KF error estimation, which includes calibration of
the IMU bias. The very large error growth due to the
bias, see Fig. 6, has been removed.
Figure 12: Kalman filter corrected INS estimate of the state
• The Kalman filter corrections to the state and updates vector (blue). State estimate plus and minus the state error
to the covariance are clearly observed at tk = k20. standard deviation (black).
• The magnitude of the state corrections is similar to the
quantification of that error by the standard deviations
extracted from the P matrix.

10
A The Discrete Time Kalman Filter B Data Description
The Kalman filter assumes a linear system model: This pdf file should come with a zipped data set. It contains
the following vaiables: t is the vector of measurement times;
zk+1 = Φk zk + Bk uk + Γk ωk (52) acc is the vector of acceleration measurements.
yk = Hk zk + ηk . (53) This is data created via simulation. It intent is to cor-
respond to the situation where an experimenter moves a 1-d
with Φk , Bk , Γk , Hk all known; a known prior distribution accelerometer in the direction of its sensitive axis, without
z0 ∼ N (ẑ0 , P0 ); a known deterministic input sequence uk ; rotation. Therefore, the continuous-time kinematic model is
and, two zero mean, white Gaussian random sequences ωk given by eqns. (1-2).
and ηk with known covariance matrix sequences Qk and Rk . That experimenter ensures that the sensor is stationary
In addition, it is assumed that ωk and ηk are uncorrelated at its original location for t = k T with T = 20 seconds.
with each other and with z0 . Therefore, defining yk = y(k T ),
For a system with the model in the previous paragraph,
the objective is to estimate the state sequence zk optimally, yk = pk + ηk , for k = 0, 1, 2, . . . .
using only the current and past measurements {yk }kj=1 .
The Kalman filter [2,3] solves this problem, see also Chap- See eqn. (8). The measurement noise corresponds to the
ter 5 in [1]. The Kalman filter has two types of updates. experimenters placement accuracy, perhaps 0.001 m.

Time Propagation: The time propagation step transports

the state estimate between the times of two measure- References
ments. The equations are:
[1] J. A. Farrell, Aided Navigation: GPS with High Rate Sen-
.
P−
k = Φk−1 P+ > >
k−1 Φk−1 + Γk−1 Qk−1 Γk−1 (54)
sors. McGraw Hill, 2008.
.
ẑ−
k = Φk−1 ẑ+
k−1 + Bk−1 uk−1 . (55) [2] R. E. Kalman, “A New Approach to Linear Filtering and
Prediction Problems,” Transactions of the ASME–Journal
Measurement Update: The measurement update step cor- of Basic Engineering, vol. 82, no. D, pp. 35–45, 1960.
rects the state estimate to account for the new infoma-
tion in the measurement yk . The equations are: [3] R. E. Kalman and R. S. Bucy, “A new approach to linear
filtering and prediction theory,” ASME Journal of Basic
.
Kk = P− k H >
k (H P
k k
− >
H k + R k )−1
(56) Engineering, Series D, vol. 83, pp. 95–108, 1961.
+ . − −
Pk = Pk − Kk Hk Pk (57)
+ . −
ẑk = ẑk + Kk rk . (58)

The derivation of these equations can be found in the many

books on optimal estimation. Those same books will present
various forms of the Kalman filter. All forms yield the same
state estimates in theory, but have different computational
and numeric properties.
Note that both the state estimate and the covariance ma-
−
trix have two values at each measurement time: ẑ+ k and ẑk
− +
and Pk and Pk . The superscript minus sign represents the
values before including yk . The superscript plus sign repre-
sents the values after including yk .
The above form of the equations assumes that the mea-
surements ŷ(t) are equally spaced at tk = kT . Equally spaced
measurements is not a requirement of the Kalman filter. It
just simplifies the presentation of the algorithm. Asynchronous
measurements are straightforward to incorporated, as long as
the time propagation step can transport the state estimate
and covariance matrix from the time of the last measurement
to the time of the most recent measurement.

11