Inertial Survey and Navigation System Assignment 4 Topic: EKF and RTS smoother toolbox

1. Name: Muhammad Irsyadi Firdaus
Student ID: P66067055
Inertial Survey and Navigation System Assignment 4
Topic: EKF and RTS smoother toolbox
The extended Kalman filter (EKF) is extends the scope of Kalman filter to
nonlinear optimal filtering problems by forming a Gaussian approximation to the joint
distribution of state 𝑥 and measurements 𝑦 using a Taylor series based transformation.
The EKF model can be described as below,
State Equation:
𝑋(𝑛 + 1) = 𝑓 (𝑋(𝑛)) + 𝑤(𝑛)
Observation Equation:
𝑍(𝑛) = 𝑔(𝑋(𝑛)) + 𝑣(𝑛)
Where
𝑋(𝑛) is the state;
𝑍(𝑛) is the measurement;
𝑤 ~ 𝑁(0, 𝑄), is Gaussian noise with covariance Q;
𝑣 ~ 𝑁(0, 𝑅), is Gaussian noise with covariance R;
𝑓() is the function for state transition, type of symbolic
expression;
𝑔() is the function for measurement, type of symbolic
expression.
The RTS-smoother (Rauch-Tung-Striebel smoother), also known as the discrete
time EKF smoother is introduced in the article to be applied for EKF estimation.
Experimental results are followed to prove that RTS-smoother can significantly improve
the performance of EKF in position estimation. The RTS - smoother to the position
estimation will be applied. It is called smoother when an estimator for the state of a
dynamic system at time t uses measurements made after time t. The accuracy of a
smoother is generally superior to that of a filter, because it uses more measurements for
its estimate. The RTS-smoother can be derived from the Kalman filter model.
In the application of the RTS-smoother to GPS position estimation, the input of
RTS-smoother are the EKF estimates of the state and its covariance of state error, which
are represented as 𝑋(𝑛) and 𝑃(𝑛) here and the outputs are defined as 𝑋𝑠 and 𝑃𝑠. The
algorithm of RTS-smoother can be described as
𝑋 𝑃(𝑛 + 1) = 𝑋(𝑛) ∗ 𝑓𝑃
𝑃𝑃(𝑛 + 1) = 𝑓𝑝 ∗ 𝑃(𝑛) ∗ 𝑓𝑃
𝑇
+ 𝑄
Where 𝑋 𝑃(𝑛 + 1) and 𝑃𝑃(𝑛 + 1) refer to the predicted state mean and state
covariance on time step 𝑛 + 1.
The difference between Kalman filter and Kalman smoother is that the recursion in
filter moves forward and in smoother backward, as can be seen from the equations
above. In smoother the recursion starts from the last time step N with
𝑋 𝑠(𝑁) = 𝑋(𝑁)
𝑃 𝑠
(𝑁) = 𝑃(𝑁)
Finally, the final estimation of receiver position can be obtained from the smoother
output as
𝑋 𝑢 = (𝑥, 𝑦, 𝑧) = (𝑋𝑠(1), 𝑋𝑠(3), 𝑋𝑠(5))

2. Figure 1. Heading (blue: filtered, red: smoothed)
Figure 2. Horizontal position (blue: filtered, red: smoothed)
Figure 3. Vertical position (blue: filtered, red: smoothed)

3. A sample of horizontal trajectory is plotted in figure 2. The filtered are clearly
more inaccurate than smoothed. As expected, the smoother produces more accurate
estimates than the filter as it uses all measurements for the estimation each time step. It
can be seen that it takes some time for the filter to respond to model transitions. In
figure 3, as one can expect, smoothing reduces this lag as well as giving a substantially
better overall performance.
Note that the difference between the smoothed and filtered estimated would be
smaller, if the measurements were more accurate, as now the filter performs rather
badly due to the great uncertainty in the measurements
By comparing the experimental results of EKF, and RTS-Smoother. The accuracy
of EKF is further improved by the application of RTS-smoother. The EKF and the RTS-
Smoother are applied to a GPS receiver. Significant improvement in position precision
can be seen from the experimental results. However, the EKF model is still imperfect,
as some biases can be more accurately estimated.
Accurate determination of GPS measurement error variance R is not an easy task.
Theoretically, satellite calendar error, multipath, atmosphere delay, tracking error,
measurement noise and other ones should all be considered. At last, when X, f,
pseudoranges, and initial values of X and P are applied to the EKF model and the
Kalman process is repeated, the information of receiver position can be obtained from
the output of the extended Kalman filter.
Figure 4. The positions on google earth
References
Liu, Hang. 2010. Two-Filter Smoothing for Accurate INS/GPS Land-Vehicle Navigation
in Urban Centers. IEEE TRANSACTIONS ON VEHICULAR TECHNOLOGY,
VOL. 59, NO. 9. pp 4256-4267
Wang, Yang. 2012. Position Estimation using Extended Kalman Filter and RTS-
Smoother in a GPS Receiver. International Congress on Image and Signal
Processing. pp 1718-1721