Previous Next sumsq_obj_ok.m

ckbs_sumsq_obj Example and Test

Source Code
 
function [ok] = sumsq_obj_ok()
    ok = true;
    % --------------------------------------------------------
    % You can change these parameters
    m    = 1;   % number of measurements per time point
    n    = 2;   % number of state vector components per time point
    N    = 3;   % number of time points
    % ---------------------------------------------------------
    %  Define the problem
    rand('seed', 123);
    x    = rand(n, N);
    z    = rand(m, N);
    h    = rand(m, N);
    g    = rand(n, N);
    dh   = zeros(m, n, N);
    dg   = zeros(n, n, N);
    qinv = zeros(n, n, N);
    rinv = zeros(m, m, N);
    for k = 1 : N
        dh(:, :, k)   = rand(m, n);
        dg(:, :, k)   = rand(n, n);
        tmp           = rand(m, m);
        rinv(:, :, k) = (tmp + tmp') / 2 + 2 * eye(m);
        tmp           = rand(n, n);
        qinv(:, :, k) = (tmp + tmp') / 2 + 2 * eye(n);
    end
    % ---------------------------------------------------------
    % Compute the Objective using ckbs_sumsq_obj
    obj  = ckbs_sumsq_obj(x, z, g, h, dg, dh, qinv, rinv);
    % ---------------------------------------------------------
    sumsq = 0;
    xk    = zeros(n, 1);
    for k = 1 : N
        xkm   = xk;
        xk    = x(:, k);
        xres  = xk      - g(:, k) - dg(:,:, k) * xkm;
        zres  = z(:, k) - h(:, k) - dh(:,:, k) * xk;
        sumsq = sumsq + xres' * qinv(:,:, k) * xres;
        sumsq = sumsq + zres' * rinv(:,:, k) * zres;
    end
    ok = ok & ( abs(obj - sumsq/2) < 1e-10 );
    return
end

Input File: example/sumsq_obj_ok.m