|
|
Previous | Next | direct_h.m |
[hk] = direct_h(k, xk, params)
[hk, Hk] = direct_h(k, xk, params)
index = params.direct_h_index
m = size(index, 1)
n = size(xk, 1)
i = 1 , ... , m
,
the mean of the i-th measurement given
xk
(the state at time index
k
) is
xk( index(i) )
n
specifying a value for
the state vector at the current time index.
xk
correspond to the direct measurements.
Each element of
index
must be between one and
n
.
hk
is a column vector of length
m
with i-th element equal to the mean of the
i-th measurement given
xk
.
Hk
is a
m x n
matrix equal to the
Jacobian of
hk
w.r.t
xk
.
function [hk, Hk] = direct_h(k, xk, params)
index = params.direct_h_index;
m = size(index, 1);
n = size(xk, 1);
if (size(xk, 2)~=1) | (size(index,2)~=1)
size_xk_2 = size(xk, 2)
size_index_2 = size(index, 2)
error('direct_h: xk or index is not a column vector')
end
if (max(index) > n) | (min(index) < 1)
max_index = max(index)
min_index = min(index)
error('direct_h: max(index) > size(xk, 1) or min(index) < 1')
end
hk = xk(index);
Hk = zeros(m, n);
for i = 1 : m
Hk( i, index(i) ) = 1;
end
return
end