|
|
Previous | Next | distance_h.m |
[hk] = distance_h(k, xk, params)
[hk, Hk] = distance_h(k, xk)
index = distance_h.index
position = distance_h.position
n = size(xk, 1)
m = size(position, 2)
i = 1 , ... , m
,
the mean of the i-th measurement given
xk
(the state at time index
k
) is
the distance from
xk
to
position(:,i)
; i.e.
norm( xk(index) - position(:, i) )
index
specifies which components
of
xk
correspond to the current moving position which the distance
measurements are made to.
It must have the same number of rows as the matrix
position
.
position
with
m
columns.
Each column specifies a fixed locations that a
distance measurements are made from.
n
specifying a value for
the state vector at the current time index.
hk
is a column vector of length
m
with i-th element equal to the mean of the
distance from
position(:,i)
to
xk(index)
.
Hk
is a
m x n
matrix equal to the
Jacobian of
hk
w.r.t
xk
.
(In the special case where one of the distances is zero,
the corresponding row of
HK
is returned as zero.)
function [hk, Hk] = distance_h(k, xk, params)
index = params.distance_h_index;
position = params.distance_h_position;
n = size(xk, 1);
m = size(position, 2);
if (size(xk, 2)~=1) | (size(index,2)~=1)
size_xk_2 = size(xk, 2)
size_index_2 = size(index, 2)
error('distance_h: xk or index is not a column vector')
end
if size(position,1) ~= size(index,1)
size_position_1 = size(position, 1)
size_index_1 = size(index, 1)
error('distance_h: position and index have different number of rows')
end
hk = zeros(m, 1);
Hk = zeros(m, n);
p_x = xk(index);
for i = 1 : m
p_i = position(:, i);
hk(i) = norm( p_x - p_i );
if hk(i) ~= 0
Hk(i, index) = (p_x - p_i) / hk(i);
end
end
return
end