Previous Next pos_vel_g.m

ckbs_nonlinear: Example Position and Velocity Transition Model

Syntax
global pos_vel_g_info
[gk    = pos_vel_g(kxk1)
[gkGk] = pos_vel_g(kxk1)

Notation

   
initial = pos_vel.initial
     
dt      = pos_vel_g.dt
     
n       = size(xk1, 1)
     
v(j)  = xk1(2 * j - 1)
     
p(j)  = xk1(2 * j)

Purpose
For j = 1 , ... , n / 2 , p(j) is a position estimate at time index k-1 , and v(j) is the corresponding velocity estimate at time index k-1 , The mean for the corresponding position at time index k , given xk1 , is
     
p(j) + v(j) * dt
. The mean for the corresponding velocity at time index k , given xk1 , is
     
v(j)
.

dt
is a scalar specifying the time between points for this Kalman smoother.

initial
is a column vector of length n specifying the initial estimate for the state vector at time index one.

k
is a positive integer scalar specifies the current time index.

xk1
is a column vector specifying a value for the state vector at the previous time index k-1 .

gk
If k == 1 , the return value gk is equal to initial . Otherwise, gk is a column vector of length n equal to the mean for the state at time index k given the value for the state at time index k-1 ; i.e., xk1 .

Gk
The return value Gk is an n x n matrix equal to the Jacobian of gk w.r.t xk1 .

Source Code
 
function [gk, Gk] = pos_vel_g(k, xk1)
	global pos_vel_g_info
	dt      = pos_vel_g_info.dt;
	initial = pos_vel_g_info.initial;
	n  = size(xk1, 1); 
	%
	if (size(xk1, 2)~=1) | (size(initial, 1)~=n) | (size(initial, 2)~=1)
		size_xk1_1     = size(xk1, 1)
		size_initial_1 = size(initial, 1)
		size_initial_2 = size(initial, 2)
		error('pos_vel_g: initial or xk1 not column vectors with same size')
	end
	%
	Gk = zeros(n, n);
	if k == 1 
		gk = initial;
		return;
	end 
	% gk(2*j-1) = xk1(2*j-1)
	Gk     = eye(n);
	for j2 = 2 : 2 : n
		% gk(2*j) = xk1(2*j) + xk1(2*j-1) * dt
		Gk(j2, j2-1) = dt;
	end
	gk = Gk * xk1;
	return
end

Input File: example/nonlinear/pos_vel_g.m