:: O-Matrix ::
 > Overview > Examples > Performance > Analysis Functions > Data Visualization > The O-Matrix Language > Data Manipulation/IO > Application Development > Using Matlab m-files

 KALMANX.OMS Script File: ```# Description: # Uses the extended Kalman-Bucy filter to track an object in the plane. # # For this problem, the state vector is {x, y, xdot, ydot} where {x, y} is # the location of the object and {xdot, ydot} is the velocity of the object. # The measurement vector, {r1, r2, r3}, is the range from three points of # known location. # clear # number of time points in the simulation const N = 10 # # three stationary points const point = [ { -1., 0.}, {0., -1.}, {0., 0.} ] # # true initial state vector const state_true_initial = { -100., -100., 20., 20.} # # the transition matrix const Phi = { ... [1., 0., 1., 0.], ... [0., 1., 0., 1.], ... [0., 0., 1., 0.], ... [0., 0., 0., 1.] ... } # # transition covariance const Q = real( diag( {5., 5., .5, .5} ) ) const Qfac = sqrt(Q) const Qinv = inv(Q) # # initial state estimate and its covariance Pini = 100. * identity(4) state_ini = state_true_initial + sqrt(Pini) * snormal(4, 1) Pinv = inv(Pini) # # data covariance const R = 2. * identity(3) const Rfac = sqrt(R) const Rinv = inv(R) # # dimension true state and data for all time points state_true = fill(0., 4, N) z = fill(0., 3, N) # # for each time point for k = 1 to N begin # # value of the true state at this time if k == 1 then ... state_true.col(k) = state_true_initial else state_true.col(k) = ... Phi * state_true.col(k - 1) + Qfac * real( snormal(4, 1) ) # # position of the object at this time pos = state_true.blk(1, k, 2, 1) # # range to each of the known points range = fill(0., 3, 1) for i = 1 to 3 begin range(i) = |pos - point.col(i)| end # # data for this time point z.col(k) = range + Rfac * real( snormal(3, 1) ) end # # transition function is linear function tran(k, statek, gstatek, dgk, Qinvk) begin gstatek = Phi * statek dgk = Phi Qinvk = Qinv end # # measurement function is nonlinear function h(state) begin # position of the object at this time pos = state.row(1, 2) # # range to each of the known points range = fill(0., 3, 1) for i = 1 to 3 begin range(i) = |pos - point.col(i)| end return range end function dh(state) begin # position of the object at this time pos = state.row(1, 2) # # range to each of the known points range = fill(0., 3, 1) for i = 1 to 3 begin range(i) = |pos - point.col(i)| end # # derivative of range with respect to position of object drdp = fill(0., 3, 2) for i = 1 to 3 begin drdp.row(i) = (pos - point.col(i))' / range(i) end # note range only depends on position, not velocity return [ drdp , fill(0., 3, 2) ] # end # # dimension estimate for all time points estimate = fill(0., rowdim(state_ini), N) # for k = 1 to N begin # model for z is h(state_ini) + H * (state - state_ini) # model for z - h(state_ini) + H * state_ini is H * state state_out = novalue P_out = novalue H = dh(state_ini) data = z.col(k) - h(state_ini) + H * state_ini # kalman(state_ini, Pini, data, R, Phi, Q, H, state_out, P_out) state_ini = state_out estimate.col(k) = state_out Pini = P_out end x = state_true.row(1)' y = state_true.row(2)' xhat = estimate.row(1)' yhat = estimate.row(2)' # # plot legend title = { ... "Extended Kalman Filter", ... "curve is truth", ... "triangles are estimates" ... } gaddtext(title, [.5, 1.], [.5, 1.]) # # make space for large title gspace(.1, .1, .1, .2) # # plot x gaddview(0., 0., .5, .9) gxtitle("x") gytitle("time") gplot(x, seq(N)) gplot(xhat, seq(N), "triangle") # # plot y gaddview(.5, 0., .5, .9) gxtitle("y") gytitle("time") gplot(y, seq(N)) gplot(yhat, seq(N), "triangle") ``` Output: ``` ```