# 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")