ckbs_nonlinear: Example Position and Velocity Transition Model

pos_vel_g.m

ckbs_nonlinear: Example Position and Velocity Transition Model
Syntax

[gk]     = pos_vel_g(k, xk1, params)

[gk, Gk] = pos_vel_g(k, xk1)

Notation



   initial = params.pos_vel_initial

      dt      = params.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, params)
    dt      = params.pos_vel_g_dt;
    initial = params.pos_vel_g_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