## Kalman filter example initiall from
## cf http://www.mathworks.com/products/matlab-coder/demos.html?file=/products/demos/shipping/coder/coderdemo_kalman_filter.html
## and used by Eddelbuettel and Sanderson in a paper on RcppArmadillo
##
## Adapted here by focusing on the RcppOctave interface relative to R
##
## Copyright (C) 2012 Dirk Eddelbuettel
##
## Released under GNU GPL (>= 2)
require(rbenchmark) # to benchmark examples
Loading required package: rbenchmarkrequire(compiler) # to byte-compile
Loading required package: compilerrequire(RcppOctave) # released to CRAN on 03 Jul 2012
## Original Matlab file
##
## % Copyright 2010 The MathWorks, Inc.
## function y = kalmanfilter(z)
## %#codegen
## dt=1;
## % Initialize state transition matrix
## A=[ 1 0 dt 0 0 0;... % [x ]
## 0 1 0 dt 0 0;... % [y ]
## 0 0 1 0 dt 0;... % [Vx]
## 0 0 0 1 0 dt;... % [Vy]
## 0 0 0 0 1 0 ;... % [Ax]
## 0 0 0 0 0 1 ]; % [Ay]
## H = [ 1 0 0 0 0 0; 0 1 0 0 0 0 ]; % Initialize measurement matrix
## Q = eye(6);
## R = 1000 * eye(2);
## persistent x_est p_est % Initial state conditions
## if isempty(x_est)
## x_est = zeros(6, 1); % x_est=[x,y,Vx,Vy,Ax,Ay]'
## p_est = zeros(6, 6);
## end
## % Predicted state and covariance
## x_prd = A * x_est;
## p_prd = A * p_est * A' + Q;
## % Estimation
## S = H * p_prd' * H' + R;
## B = H * p_prd';
## klm_gain = (S \ B)';
## % Estimated state and covariance
## x_est = x_prd + klm_gain * (z - H * x_prd);
## p_est = p_prd - klm_gain * H * p_prd;
## % Compute the estimated measurements
## y = H * x_est;
## end % of the function
## Direct translation of Matlab code
##
FirstKalmanR <- function(pos) {
kalmanfilter <- function(z) {
dt <- 1
A <- matrix( c( 1, 0, dt, 0, 0, 0, # x
0, 1, 0, dt, 0, 0, # y
0, 0, 1, 0, dt, 0, # Vx
0, 0, 0, 1, 0, dt, # Vy
0, 0, 0, 0, 1, 0, # Ax
0, 0, 0, 0, 0, 1), # Ay
6, 6, byrow=TRUE)
H <- matrix( c(1, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0),
2, 6, byrow=TRUE)
Q <- diag(6)
R <- 1000 * diag(2)
## predicted state and covriance
xprd <- A %*% xest
pprd <- A %*% pest %*% t(A) + Q
## estimation
S <- H %*% t(pprd) %*% t(H) + R
B <- H %*% t(pprd)
## kalmangain <- (S \ B)'
kalmangain <- t(solve(S, B))
## estimated state and covariance, assign to vars in parent env
xest <<- xprd + kalmangain %*% (z - H %*% xprd)
pest <<- pprd - kalmangain %*% H %*% pprd
## compute the estimated measurements
y <- H %*% xest
}
xest <- matrix(0, 6, 1)
pest <- matrix(0, 6, 6)
N <- nrow(pos)
y <- matrix(NA, N, 2)
for (i in 1:N) {
y[i,] <- kalmanfilter(t(pos[i,,drop=FALSE]))
}
invisible(y)
}
## this is a rewrite which splits the per-observation loop and the variable initialization
## the 'persistent' variable is in the enclosing scope and local to KalmanR
##
## this version is shown in the paper
##
KalmanR <- function(pos) {
kalmanfilter <- function(z) {
## predicted state and covriance
xprd <- A %*% xest
pprd <- A %*% pest %*% t(A) + Q
## estimation
S <- H %*% t(pprd) %*% t(H) + R
B <- H %*% t(pprd)
## kalmangain <- (S \ B)'
kalmangain <- t(solve(S, B))
## estimated state and covariance, assign to vars in parent env
xest <<- xprd + kalmangain %*% (z - H %*% xprd)
pest <<- pprd - kalmangain %*% H %*% pprd
## compute the estimated measurements
y <- H %*% xest
}
dt <- 1
A <- matrix( c( 1, 0, dt, 0, 0, 0, # x
0, 1, 0, dt, 0, 0, # y
0, 0, 1, 0, dt, 0, # Vx
0, 0, 0, 1, 0, dt, # Vy
0, 0, 0, 0, 1, 0, # Ax
0, 0, 0, 0, 0, 1), # Ay
6, 6, byrow=TRUE)
H <- matrix( c(1, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0),
2, 6, byrow=TRUE)
Q <- diag(6)
R <- 1000 * diag(2)
N <- nrow(pos)
y <- matrix(NA, N, 2)
xest <- matrix(0, 6, 1)
pest <- matrix(0, 6, 6)
for (i in 1:N) {
y[i,] <- kalmanfilter(t(pos[i,,drop=FALSE]))
}
invisible(y)
}
KalmanRfun <- function(pos) {
dt <- 1
A <- matrix( c( 1, 0, dt, 0, 0, 0, # x
0, 1, 0, dt, 0, 0, # y
0, 0, 1, 0, dt, 0, # Vx
0, 0, 0, 1, 0, dt, # Vy
0, 0, 0, 0, 1, 0, # Ax
0, 0, 0, 0, 0, 1), # Ay
6, 6, byrow=TRUE)
H <- matrix( c(1, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0),
2, 6, byrow=TRUE)
Q <- diag(6)
R <- 1000 * diag(2)
N <- nrow(pos)
y <- matrix(NA, N, 2)
xest <- matrix(0, 6, 1)
pest <- matrix(0, 6, 6)
for (i in 1:N) {
z <- t(pos[i,,drop=FALSE])
## predicted state and covriance
xprd <- A %*% xest
pprd <- A %*% pest %*% t(A) + Q
## estimation
S <- H %*% t(pprd) %*% t(H) + R
B <- H %*% t(pprd)
## kalmangain <- (S \ B)'
kalmangain <- t(solve(S, B))
## estimated state and covariance, assign to vars in parent env
xest <- xprd + kalmangain %*% (z - H %*% xprd)
pest <- pprd - kalmangain %*% H %*% pprd
## compute the estimated measurements
y[i,] <- H %*% xest
}
invisible(y)
}
## define Octave function
KalmanOctave <- OctaveFunction("function [Y] = kalmanM(pos)
dt=1;
%% Initialize state transition matrix
A=[ 1 0 dt 0 0 0;... % [x ]
0 1 0 dt 0 0;... % [y ]
0 0 1 0 dt 0;... % [Vx]
0 0 0 1 0 dt;... % [Vy]
0 0 0 0 1 0 ;... % [Ax]
0 0 0 0 0 1 ]; % [Ay]
H = [ 1 0 0 0 0 0; 0 1 0 0 0 0 ]; % Initialize measurement matrix
Q = eye(6);
R = 1000 * eye(2);
x_est = zeros(6, 1); % x_est=[x,y,Vx,Vy,Ax,Ay]'
p_est = zeros(6, 6);
numPts = size(pos,1);
Y = zeros(numPts, 2);
for idx = 1:numPts
z = pos(idx, :)';
%% Predicted state and covariance
x_prd = A * x_est;
p_prd = A * p_est * A' + Q;
%% Estimation
S = H * p_prd' * H' + R;
B = H * p_prd';
klm_gain = (S \\ B)'; % double backslash because of embedded code
%% Estimated state and covariance
x_est = x_prd + klm_gain * (z - H * x_prd);
p_est = p_prd - klm_gain * H * p_prd;
%% Compute the estimated measurements
Y(idx, :) = H * x_est;
end % of the function
end
")
FirstKalmanRC <- cmpfun(FirstKalmanR)
KalmanRC <- cmpfun(KalmanR)
KalmanRfunC <- cmpfun(KalmanRfun)
## Read data
pos <- as.matrix(read.table(tc <- textConnection("-4.76074274e-01 8.27344982e-01
-4.66073333e-01 8.25346801e-01
-4.56071632e-01 8.21349631e-01
-4.46068539e-01 8.15353570e-01
-4.36063422e-01 8.07358714e-01
-4.26055649e-01 7.97365162e-01
-4.16044587e-01 7.85373011e-01
-4.06029604e-01 7.71382359e-01
-3.96010067e-01 7.55393303e-01
-3.85985344e-01 7.37405941e-01
-3.75954804e-01 7.17420370e-01
-3.65917812e-01 6.95436687e-01
-3.55873738e-01 6.71454991e-01
-3.45821948e-01 6.45475379e-01
-3.35761811e-01 6.17497948e-01
-3.25692694e-01 5.87522796e-01
-3.15613964e-01 5.55550021e-01
-3.05524990e-01 5.21579720e-01
-2.95425138e-01 4.85611990e-01
-2.85313778e-01 4.47646929e-01
-2.75190275e-01 4.07684635e-01
-2.65053998e-01 3.65725205e-01
-2.54904315e-01 3.21768737e-01
-2.44740592e-01 2.75815328e-01
-2.34562199e-01 2.27865076e-01
-2.24368501e-01 1.77918079e-01
-2.14158868e-01 1.25974433e-01
-2.03932667e-01 7.20342366e-02
-1.93689265e-01 1.60975872e-02
-1.83428030e-01 -4.18354177e-02
-1.73148329e-01 -1.01764681e-01
-1.62849531e-01 -1.63690104e-01
-1.52531002e-01 -2.27611590e-01
-1.42192111e-01 -2.93529041e-01
-1.31832225e-01 -3.61442361e-01
-1.21450712e-01 -4.31351450e-01
-1.11046940e-01 -5.03256212e-01
-1.00620275e-01 -5.77156550e-01
-9.01700865e-02 -6.53052365e-01
-7.96957410e-02 -7.30943560e-01
-6.91966066e-02 -8.10830038e-01
-5.86720509e-02 -8.92711700e-01
-4.81214414e-02 -9.68723398e-01
-3.75441458e-02 -8.82849846e-01
-2.69395318e-02 -7.98971187e-01
-1.63069671e-02 -7.17087322e-01
-5.64581912e-03 -6.37198155e-01
5.04454433e-03 -5.59303588e-01
1.57647557e-02 -4.83403523e-01
2.65154472e-02 -4.09497862e-01
3.72972513e-02 -3.37586509e-01
4.81108004e-02 -2.67669365e-01
5.89567268e-02 -1.99746334e-01
6.98356629e-02 -1.33817317e-01
8.07482410e-02 -6.98822165e-02
9.16950935e-02 -7.94093586e-03
1.02676853e-01 5.20066229e-02
1.13694151e-01 1.09960557e-01
1.24747621e-01 1.65920965e-01
1.35837894e-01 2.19887943e-01
1.46965604e-01 2.71861590e-01
1.58131383e-01 3.21842002e-01
1.69335862e-01 3.69829278e-01
1.80579674e-01 4.15823514e-01
1.91863452e-01 4.59824809e-01
2.03187829e-01 5.01833260e-01
2.14553435e-01 5.41848965e-01
2.25960904e-01 5.79872020e-01
2.37410868e-01 6.15902524e-01
2.48903960e-01 6.49940575e-01
2.60440811e-01 6.81986269e-01
2.72022055e-01 7.12039704e-01
2.83648323e-01 7.40100978e-01
2.95320248e-01 7.66170188e-01
3.07038462e-01 7.90247432e-01
3.18803597e-01 8.12332807e-01
3.30616287e-01 8.32426411e-01
3.42477163e-01 8.50528342e-01
3.54386857e-01 8.66638696e-01
3.66346003e-01 8.80757573e-01
3.78355232e-01 8.92885068e-01
3.90415176e-01 9.03021280e-01
4.02526469e-01 9.11166306e-01
4.14689742e-01 9.17320243e-01
4.26905628e-01 9.21483190e-01
4.39174759e-01 9.23655244e-01
4.51497768e-01 9.23836502e-01
4.63875286e-01 9.22027062e-01
4.76307947e-01 9.18227021e-01
4.88796383e-01 9.12436477e-01
5.01341225e-01 9.04655528e-01
5.13943106e-01 8.94884271e-01
5.26602660e-01 8.83122803e-01
5.39320517e-01 8.69371222e-01
5.52097311e-01 8.53629626e-01
5.64933673e-01 8.35898112e-01
5.77830237e-01 8.16176778e-01
5.90787634e-01 7.94465721e-01
6.03806497e-01 7.70765039e-01
6.16887458e-01 7.45074829e-01
6.16887458e-01 7.45074829e-01
2.43647025e-01 3.48142367e-01
-1.29593408e-01 -4.87900946e-02
-5.02833841e-01 -4.45722556e-01
-8.76074274e-01 -8.42655018e-01
-8.76074274e-01 -8.42655018e-01
-8.66073333e-01 -8.14653199e-01
-8.56071632e-01 -7.88650369e-01
-8.46068539e-01 -7.64646430e-01
-8.36063422e-01 -7.42641286e-01
-8.26055649e-01 -7.22634838e-01
-8.16044587e-01 -7.04626989e-01
-8.06029604e-01 -6.88617641e-01
-7.96010067e-01 -6.74606697e-01
-7.85985344e-01 -6.62594059e-01
-7.75954804e-01 -6.52579630e-01
-7.65917812e-01 -6.44563313e-01
-7.55873738e-01 -6.38545009e-01
-7.45821948e-01 -6.34524621e-01
-7.35761811e-01 -6.32502052e-01
-7.25692694e-01 -6.32477204e-01
-7.15613964e-01 -6.34449979e-01
-7.05524990e-01 -6.38420280e-01
-6.95425138e-01 -6.44388010e-01
-6.85313778e-01 -6.52353071e-01
-6.75190275e-01 -6.62315365e-01
-6.65053998e-01 -6.74274795e-01
-6.54904315e-01 -6.88231263e-01
-6.44740592e-01 -7.04184672e-01
-6.34562199e-01 -7.22134924e-01
-6.24368501e-01 -7.42081921e-01
-6.14158868e-01 -7.64025567e-01
-6.03932667e-01 -7.87965763e-01
-5.93689265e-01 -8.13902413e-01
-5.83428030e-01 -8.41835418e-01
-5.73148329e-01 -8.71764681e-01
-5.62849531e-01 -9.03690104e-01
-5.52531002e-01 -9.37611590e-01
-5.42192111e-01 -9.71782807e-01
-5.31832225e-01 -9.33867676e-01
-5.21450712e-01 -8.97948315e-01
-5.11046940e-01 -8.64024627e-01
-5.00620275e-01 -8.32096515e-01
-4.90170086e-01 -8.02163879e-01
-4.79695741e-01 -7.74226624e-01
-4.69196607e-01 -7.48284652e-01
-4.58672051e-01 -7.24337865e-01
-4.48121441e-01 -7.02386165e-01
-4.37544146e-01 -6.82429455e-01
-4.26939532e-01 -6.64467637e-01
-4.16306967e-01 -6.48500614e-01
-4.05645819e-01 -6.34528289e-01
-3.94955456e-01 -6.22550563e-01
-3.84235244e-01 -6.12567339e-01
-3.73484553e-01 -6.04578520e-01
-3.62702749e-01 -5.98584009e-01
-3.51889200e-01 -5.94583707e-01
-3.41043273e-01 -5.92577517e-01
-3.30164337e-01 -5.92565341e-01
-3.19251759e-01 -5.94547083e-01
-3.08304907e-01 -5.98522644e-01
-2.97323147e-01 -6.04491926e-01
-2.86305849e-01 -6.12454834e-01
-2.75252379e-01 -6.22411268e-01
-2.64162106e-01 -6.34361131e-01
-2.53034396e-01 -6.48304326e-01
-2.41868617e-01 -6.64240755e-01
-2.30664138e-01 -6.82170321e-01
-2.19420326e-01 -7.02092926e-01
-2.08136548e-01 -7.24008473e-01
-1.96812171e-01 -7.47916863e-01
-1.85446565e-01 -7.73818000e-01
-1.74039096e-01 -8.01711787e-01
-1.62589132e-01 -8.31598124e-01
-1.51096040e-01 -8.63476915e-01
-1.39559189e-01 -8.97348063e-01
-1.27977945e-01 -9.33211470e-01
-1.16351677e-01 -9.71067037e-01
-1.04679752e-01 -9.34397179e-01
-9.29615383e-02 -8.92555770e-01
-8.11964026e-02 -8.52706229e-01
-6.93837130e-02 -8.14848460e-01
-5.75228372e-02 -7.78982364e-01
-4.56131427e-02 -7.45107844e-01
-3.36539972e-02 -7.13224802e-01
-2.16447683e-02 -6.83333142e-01
-9.58482365e-03 -6.55432765e-01
2.52646905e-03 -6.29523574e-01
1.46897422e-02 -6.05605471e-01
2.69056281e-02 -5.83678358e-01
3.91747593e-02 -5.63742139e-01
5.14977679e-02 -5.45796716e-01
6.38752864e-02 -5.29841991e-01
7.63079472e-02 -5.15877866e-01
8.87963825e-02 -5.03904245e-01
1.01341225e-01 -4.93921029e-01
1.13943106e-01 -4.85928121e-01
1.26602660e-01 -4.79925423e-01
1.39320517e-01 -4.75912839e-01
1.52097311e-01 -4.73890270e-01
1.64933673e-01 -4.73857618e-01
1.77830237e-01 -4.75814787e-01
1.90787634e-01 -4.79761679e-01
2.03806497e-01 -4.85698196e-01
2.16887458e-01 -4.93624240e-01
2.16887458e-01 -4.93624240e-01
4.01147025e-01 -1.33381935e-01
5.85406592e-01 2.26860371e-01
7.69666159e-01 5.87102676e-01
9.53925726e-01 9.47344982e-01
9.53925726e-01 9.47344982e-01
9.43926667e-01 9.15346801e-01
9.33928368e-01 8.81349631e-01
9.23931461e-01 8.45353570e-01
9.13936578e-01 8.07358714e-01
9.03944351e-01 7.67365162e-01
8.93955413e-01 7.25373011e-01
8.83970396e-01 6.81382359e-01
8.73989933e-01 6.35393303e-01
8.64014656e-01 5.87405941e-01
8.54045196e-01 5.37420370e-01
8.44082188e-01 4.85436687e-01
8.34126262e-01 4.31454991e-01
8.24178052e-01 3.75475379e-01
8.14238189e-01 3.17497948e-01
8.04307306e-01 2.57522796e-01
7.94386036e-01 1.95550021e-01
7.84475010e-01 1.31579720e-01
7.74574862e-01 6.56119899e-02
7.64686222e-01 -2.35307073e-03
7.54809725e-01 -7.23153647e-02
7.44946002e-01 -1.44274795e-01
7.35095685e-01 -2.18231263e-01
7.25259408e-01 -2.94184672e-01
7.15437801e-01 -3.72134924e-01
7.05631499e-01 -4.52081921e-01
6.95841132e-01 -5.34025567e-01
6.86067333e-01 -6.17965763e-01
6.76310735e-01 -7.03902413e-01
6.66571970e-01 -7.91835418e-01
6.56851671e-01 -8.81764681e-01
6.47150469e-01 -9.71621744e-01
6.37468998e-01 -8.77698446e-01
6.27807889e-01 -7.85771114e-01
6.18167775e-01 -6.95839649e-01
6.08549288e-01 -6.07903955e-01
5.98953060e-01 -5.21963934e-01
5.89379725e-01 -4.38019487e-01
5.79829914e-01 -3.56070518e-01
5.70304259e-01 -2.76116929e-01
5.60803393e-01 -1.98158623e-01
5.51327949e-01 -1.22195502e-01
5.41878559e-01 -4.82274683e-02
5.32455854e-01 2.37455755e-02
5.23060468e-01 9.37237269e-02
5.13693033e-01 1.61707083e-01
5.04354181e-01 2.27695743e-01
4.95044544e-01 2.91689802e-01
4.85764756e-01 3.53689360e-01
4.76515447e-01 4.13694512e-01
4.67297251e-01 4.71705358e-01
4.58110800e-01 5.27721993e-01
4.48956727e-01 5.81744517e-01
4.39835663e-01 6.33773026e-01
4.30748241e-01 6.83807619e-01
4.21695093e-01 7.31848392e-01
4.12676853e-01 7.77895442e-01
4.03694151e-01 8.21948869e-01
3.94747621e-01 8.64008769e-01
3.85837894e-01 9.04075239e-01
3.76965604e-01 9.42148378e-01
3.68131383e-01 9.78228282e-01
3.59335862e-01 1.01231505e+00
3.50579674e-01 1.01027937e+00
3.41863452e-01 9.80180397e-01
3.33187829e-01 9.48088578e-01
3.24553435e-01 9.14004012e-01
3.15960904e-01 8.77926796e-01
3.07410868e-01 8.39857030e-01
2.98903960e-01 7.99794810e-01
2.90440811e-01 7.57740233e-01
2.82022055e-01 7.13693397e-01
2.73648323e-01 6.67654401e-01
2.65320248e-01 6.19623340e-01
2.57038462e-01 5.69600313e-01
2.48803597e-01 5.17585418e-01
2.40616287e-01 4.63578752e-01
2.32477163e-01 4.07580412e-01
2.24386857e-01 3.49590496e-01
2.16346003e-01 2.89609102e-01
2.08355232e-01 2.27636326e-01
2.00415176e-01 1.63672267e-01
1.92526469e-01 9.77170229e-02
1.84689742e-01 2.97706900e-02
1.76905628e-01 -4.01666337e-02
1.69174759e-01 -1.12094851e-01
1.61497768e-01 -1.86013863e-01
1.53875286e-01 -2.61923574e-01
1.46307947e-01 -3.39823885e-01
1.38796383e-01 -4.19714700e-01
1.31341225e-01 -5.01595920e-01
1.23943106e-01 -5.85467448e-01
1.16602660e-01 -6.71329186e-01
1.09320517e-01 -7.59181037e-01
1.02097311e-01 -8.49022904e-01
9.49336734e-02 -9.40854688e-01
8.78302370e-02 -9.10635555e-01
8.07876341e-02 -8.14822416e-01
7.38064970e-02 -7.20998902e-01
6.68874582e-02 -6.29164916e-01"), header=FALSE, col.names=c("x","y")))
stopifnot(all.equal(KalmanR(pos), KalmanRC(pos)),
all.equal(KalmanR(pos), FirstKalmanR(pos)),
all.equal(KalmanR(pos), FirstKalmanRC(pos)),
all.equal(KalmanR(pos), KalmanRfun(pos)),
all.equal(KalmanR(pos), KalmanRfunC(pos)),
all.equal(KalmanR(pos), KalmanOctave(pos))
)
res <- benchmark(KalmanR(pos),
KalmanRC(pos),
KalmanRfun(pos),
KalmanRfunC(pos),
FirstKalmanR(pos),
FirstKalmanRC(pos),
KalmanOctave(pos),
columns = c("test", "replications", "elapsed", "relative"),
order="relative",
replications=100)
print(res)
test replications elapsed relative
7 KalmanOctave(pos) 100 2.059 1.000
4 KalmanRfunC(pos) 100 3.261 1.584
2 KalmanRC(pos) 100 3.628 1.762
1 KalmanR(pos) 100 3.999 1.942
6 FirstKalmanRC(pos) 100 4.071 1.977
3 KalmanRfun(pos) 100 4.142 2.012
5 FirstKalmanR(pos) 100 4.534 2.202
#plot(pos, col='blue', pch=20)
#points(KalmanCpp(pos), col='green', pch=20)
#plot(pos, col='blue', pch=20)
#points(KalmanR(pos), col='green', pch=20)
#R> dput(brewer.pal(4, 'Set1'))
#c("#E41A1C", "#377EB8", "#4DAF4A", "#984EA3")
#plot(pos, col='#377EB8', pch=20)
#points(KalmanR(pos), col="#E41A1C", pch=20)
#R> dput(brewer.pal(4, 'Paired'))
#c("#A6CEE3", "#1F78B4", "#B2DF8A", "#33A02C")
## doPdfPlot <- function() {
## pdf("../images/kalmanExample.pdf")
## plot(pos, col='#A6CEE3DD', pch=18,
## main="Object Trajectory and Kalman Filter Estimate")
## op <- par(mar=c(4,4,1,1))
## plot(pos, col='#A6CEE3DD', pch=22)
## points(KalmanR(pos), col="#1F78B4DD", pch=20)
## legend("topleft", c("Trajectory", "Estimate"), bty="n",
## col=c("#A6CEE3DD", "#1F78B4DD"), pch=c(22,20), cex=0.9)
## par(op)
## dev.off()
## }
## doEpsPlot <- function() {
## setEPS()
## postscript("../images/kalmanExample.eps")
## op <- par(mar=c(4,4,1,1))
## plot(pos, col='#A6CEE3', pch=22)
## points(KalmanR(pos), col="#1F78B4", pch=20)
## legend("topleft", c("Trajectory", "Estimate"), bty="n",
## col=c("#A6CEE3", "#1F78B4"), pch=c(22,20), cex=0.9)
## par(op)
## dev.off()
## }