Index-> contents reference index search external Previous Next Up-> ckbs ckbs_nonlinear nonlinear_utility distance_h.m ckbs-> license ckbs_t_general ckbs_nonlinear ckbs_L1_nonlinear ckbs_affine ckbs_affine_singular ckbs_L1_affine utility all_ok.m whatsnew wishlist bib ckbs_nonlinear-> get_started_ok.m sine_wave_ok.m vanderpol_ok.m nonlinear_utility nonlinear_utility-> box_f.m direct_h.m distance_h.m no_f.m persist_g.m pos_vel_g.m sine_f.m distance_h.m Headings-> Syntax Notation Purpose index position k xk hk Hk Source Code

ckbs_nonlinear: Example of Distance Measurement Model

Syntax
```[hk]     = distance_h(k, xk, params) ``` ```[hk, Hk] = distance_h(k, xk) ```
Notation
```      index     = distance_h.index      position  = distance_h.position      n         = size(xk, 1)      m         = size(position, 2) ```
Purpose
For `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
the integer column vector `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
The matrix `position` with `m` columns. Each column specifies a fixed locations that a distance measurements are made from.

k
is an integer scalar specifying the time index (not used).

xk
is a column vector with length `n` specifying a value for the state vector at the current time index.

hk
The return value `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
The return value `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.)

Source Code ``` 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 ```
Input File: example/nonlinear/distance_h.m