demo/Kalman.R

## 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
require(compiler)			# to byte-compile

require(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)


#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()
## }
renozao/RcppOctave documentation built on June 30, 2017, 2:11 p.m.