# source:projects/ckbs/trunk/test/nonlinear_ok_box_h.m@95

Last change on this file since 95 was 35, checked in by bradbell, 11 years ago

File size: 1.0 KB
Line
1% -------------------------------------------------------------------
2% ckbs: Constrained Kalman-Bucy Smoother Program: Copyright (C) 2006
4%          Gianluigi Pillonetto: giapi at dei dot unipd dot it
6% -------------------------------------------------------------------
7% \$begin nonlinear_ok_box_h.m\$\$ \$newlinech %\$\$
8%
9% \$spell
10%       ckbs
11%       hk
12%       xk
13%       sqrt
14%       dr
15% \$\$
16%
17% \$section Example Measurement Function: nonlinear_ok_box_h\$\$
18%
20% \$newlinech \$\$ \$codep
21function [hk, Hk] = nonlinear_ok_box_h(k, xk)
22        global nonlinear_ok_box_s1
23        global nonlinear_ok_box_s2
24        s1  = nonlinear_ok_box_s1;
25        s2  = nonlinear_ok_box_s2;
26        r1  = sqrt( (xk(2) - s1(1))^2 + (xk(4) - s1(2))^2 );
27        r2  = sqrt( (xk(2) - s2(1))^2 + (xk(4) - s2(2))^2 );
28        hk = [ r1 ; r2 ];
29        dr1 = [ 0, xk(2) - s1(1), 0, xk(4) - s1(2) ] / r1;
30        dr2 = [ 0, xk(2) - s2(1), 0, xk(4) - s2(2) ] / r2;
31        Hk = [ dr1 ; dr2 ];
32        return
33end
34% \$\$ \$newlinech %\$\$
35% \$end
Note: See TracBrowser for help on using the repository browser.