Index-> contents reference index search external Previous Next Up-> ckbs ckbs_nonlinear 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 Headings-> Syntax Purpose Notation Problem First Order Conditions f_fun ---..no_f ---..k ---..xk ---..fk ---..Fk g_fun ---..k ---..xk1 ---..gk ---..Gk h_fun ---..k ---..xk ---..hk ---..Hk max_itr epsilon x_in z qinv rinv level x_out u_out info ---..max_f ---..max_grad ---..max_fu ---..obj_cur ---..lambda ---..lam_aff ---..Penalty Parameter Example ---..Simple ---..Unconstrained ---..Box Constraints ---..Nonlinear Constraints ---..Van Der Pol Oscillator

The Nonlinear Constrained Kalman-Bucy Smoother

Syntax
[x_out, u_out, info] = ckbs_nonlinear(       f_fun, g_fun, h_fun, ...       max_itr, epsilon, x_in, z, qinv, rinv, level)

Purpose
This routine minimizes the Kalman-Bucy smoother residual sum of squares objective for a general nonlinear model functions (in the case where the model functions are affine, the routine ckbs_affine is more efficient).

Notation
The Kalman-Bucy smoother residual sum of squares is defined by   $\begin{array}{rcl} S ( x_1 , \ldots , x_N ) & = & \sum_{k=1}^N S_k ( x_k , x_{k-1} ) \\ S_k ( x_k , x_{k-1} ) & = & \frac{1}{2} [ z_k - h_k ( x_k ) ]^\R{T} * R_k^{-1} * [ z_k - h_k ( x_k ) ] \\ & + & \frac{1}{2} [ x_k - g_k ( x_{k-1} ) ]^\R{T} * Q_k^{-1} * [ x_k - g_k ( x_{k-1} ) ] \\ \end{array}$  where the matrices   R_k and   Q_k are symmetric positive definite and   x_0 is the constant zero. Note that   g_1 ( x_0 ) is the initial state estimate and   Q_1 is the corresponding covariance.

Problem
Our constrained Kalman-Bucy smoother problem is   $\begin{array}{rll} {\rm minimize} & S( x_1 , \ldots , x_N ) & {\rm w.r.t.} \; x_1 \in \B{R}^n , \ldots , x_N \in \B{R}^n \\ {\rm subject \; to} & f_k(x_k) \leq 0 & {\rm for} \; k = 1 , \ldots , N \end{array}$ 

First Order Conditions
A state sequence   ( x_1 , \ldots , x_N ) is considered a solution if there is a Lagrange multiplier sequence   ( u_1 , \ldots , u_N ) such that the following conditions are satisfied for   i = 1 , \ldots , \ell and   k = 1 , \ldots , N :   $\begin{array}{rcl} f_k ( x_k ) & \leq & \varepsilon \\ 0 & \leq & u_k \\ | u_k^\R{T} * \partial_k f_k ( x_k ) + \partial_k S( x_1 , \ldots , x_N ) | & \leq & \varepsilon \\ | f_k ( x_k )_i | * ( u_k )_i & \leq & \varepsilon \end{array}$  Here the notation   \partial_k is used for the partial derivative of with respect to   x_k and the notation   (u_k)_i denotes the i-th component of   u_k .

f_fun
The ckbs_nonlinear argument f_fun is a function that supports both of the following syntaxes        [fk] = feval(f_fun, k, xk)       [fk, Fk] = feval(f_fun, k, xk) 
no_f
In the case where there are no constraints, one can use no_f.m which is equivalent to  [fk, Fk] = f_fun(k, xk) n = size(xk, 1); fk = -1; Fk = zeros(1, n); return end 

k
The f_fun argument k is an integer with   1 \leq k \leq N .

xk
The f_fun argument xk is an column vector with length   n . It specifies the state vector at time index k   $xk = x_k$  .

fk
The f_fun result fk is an column vector with length   \ell and   $fk = f_k ( x_k )$ 

Fk
If the f_fun result Fk is present in the syntax, it is the   \ell \times n matrix given by and   $Fk = \partial_k f_k ( x_k )$ 

g_fun
The ckbs_nonlinear argument g_fun is a function that supports both of the following syntaxes        [gk] = feval(g_fun, k, xk1)       [gk, Gk] = feval(g_fun, k, xk1) 
k
The g_fun argument k is an integer with   1 \leq k \leq N . The case   k = 1 serves to specify the initial state estimate.

xk1
The g_fun argument xk1 is an column vector with length   n . It specifies the state vector at time index k   $xk1 = x_{k-1}$  . In the case   k = 1 , the value of xk1 does not matter.

gk
The g_fun result gk is an column vector with length   n and   $gk = g_k ( x_{k-1} )$  In the case   k = 1 , the value of gk is the initial state estimate at time index k .

Gk
If the g_fun result Gk is present in the syntax, it is the   n \times n matrix given by and   $Gk = \partial_{k-1} g_k ( x_{k-1} )$  In the case   k = 1 , the value of Gk is the zero matrix; i.e.,   g_k does not depend on the value of   x_{k-1} .

h_fun
The ckbs_nonlinear argument h_fun is a function that supports both of the following syntaxes        [hk] = feval(h_fun, k, xk)       [hk, Hk] = feval(h_fun, k, xk) 
k
The h_fun argument k is an integer with   1 \leq k \leq N .

xk
The h_fun argument xk is an column vector with length   n . It specifies the state vector at time index k   $xk = x_k$  .

hk
The h_fun result hk is an column vector with length   m and   $hk = h_k ( x_k )$ 

Hk
If the h_fun result Hk is present in the syntax, it is the   m \times n matrix given by and   $Hk = \partial_k h_k ( x_k )$ 

max_itr
The integer scalar max_itr specifies the maximum number of iterations of the algorithm to execute. It must be greater than or equal to zero. Note that if it is zero, the first row of the info return value will still be computed. This can be useful for deciding what is a good value for the argument epsilon .

epsilon
The ckbs_nonlinear argument epsilon is a positive scalar. It specifies the convergence criteria value; i.e.,   $\varepsilon = epsilon$  In some instances, ckbs_nonlinear will return after printing a warning without convergence; see info .

x_in
The ckbs_nonlinear argument x_in is a two dimensional array with size   n \times N . It specifies a sequence of state values; i.e., for   k = 1 , \ldots , N   $x\_in (:, k) = x_k$  The closer the initial state sequence is to the solution the faster, and more likely, the ckbs_nonlinear will converge. The initial state sequence need not be feasible; i.e. it is not necessary that   $f_k ( x_k ) \leq 0$  for all   k .

z
The ckbs_nonlinear argument z is a two dimensional array with size   m \times N . It specifies the sequence of measurement vectors; i.e., for   k = 1 , \ldots , N   $z(:, k) = z_k$

qinv
The ckbs_nonlinear argument qinv is a three dimensional array with size   n \times n \times N . It specifies the inverse of the variance of the measurement noise; i.e., for   k = 1 , \ldots , N   $qinv(:,:, k) = Q_k^{-1}$ In the case   k = 1 , the value of   Q_k is the variance of the initial state estimate (see g_fun .

rinv
The ckbs_nonlinear argument rinv is a three dimensional array, with size   m \times m \times N . it specifies the inverse of the variance of the transition noise; i.e., for   k = 1 , \ldots , N   $rinv(:,:, k) = R_k^{-1}$ It is ok to signify a missing data value by setting the corresponding row and column of rinv to zero. This also enables use to interpolate the state vector   x_k to points where there are no measurements.

level
The integer scalar level specifies the level of tracing to do. If level == 0 , no tracing is done. If level >= 1 , the row index q and the correspond row of info are printed as soon as soon as they are computed. If level >= 2 , a check of the derivative calculations is printed before the calculations start. In this case, control will return to the keyboard so that you can print values, continue, or abort the calculation.

x_out
The result x_out is a two dimensional array with size   n \times N .   ( x_1 , \ldots , x_N ) . It contains an approximation for the optimal sequence; i.e., for   k = 1 , \ldots , N   $x\_out(:, k) = x_k$

u_out
The result u_out is a two dimensional array with size %   \ell \times N . It contains the Lagrange multiplier sequence corresponding to x_out ; i.e., for   k = 1 , \ldots , N   $u\_out(:, k) = u_k$ The pair x_out , u_out satisfy the first order conditions .

info
The result info is a matrix with each row corresponding to an iteration of the algorithm. Note that ckbs_nonlinear has satisfied the convergence condition if and only if        all( info(end, 1:3) <= epsilon )   We use   \{ x_k^q \} to denote the state vector sequence at the at the end of iteration   q-1 and   \{ u_k^q \} for the dual vector sequence (  u_k^q \geq 0 ).

max_f
The value        max_f(q) = info(q, 1)  is a bound on the constraint functions at the end of iteration q-1 . To be specific   $max\_f(q) = \max_{i, k} [ f_k ( x_k^q )_i ]$  where the maximum is with respect to   i = 1 , \ldots , \ell and   k = 1 , \ldots , N . Convergence requires this value to be less than or equal epsilon .

The value        max_grad(q) = info(q, 2)  is a bound on the partial of the Lagrangian with respect to   x at the end of iteration q-1 . To be specific   $max\_grad(q) = \max_{j, k} \left[ \left| (u_k^q)^\R{T} * \partial_k f_k(x_k^q) + \partial_k S(x_1^q, \ldots ,x_N^q) \right| \right]_j$  where the maximum is with respect to   j = 1 , \ldots , n and   k = 1 , \ldots , N . This value should converge to zero.

max_fu
The value        max_fu = info(q, 3)  is a bound on the complementarity conditions at the end of iteration q-1 . To be specific   $max\_fu(q) = \max_{i, k} [ | f_k ( x_k )_i | * ( u_k )_i ]$  where the maximum is with respect to   i = 1 , \ldots , \ell and   k = 1 , \ldots , N . This value should converge to be less than or equal zero.

obj_cur
The value        obj_cur(q) = info(q, 4)  is the value of the objective function at the end of   q-1 ; i.e.,   $obj\_cur(q) = S( x_1^q , \ldots , x_k^q )$  (Note that   info(1, 4)% is the value of the objective corresponding to x_in .

lambda
The value        lambda(q) = info(q, 5)  is the line search step size used during iteration   q-1 of ckbs_nonlinear. If the problem is nearly affine (if the affine approximate is accurate) this will be one. Much small step sizes indicate highly non-affine problems (with the exception that info(1, 5) is always zero).

lam_aff
The value        lam_aff(q) = info(q, 6)  is the minimum line search step size while solving the affine sub-problem that corresponds to iteration   q-1 of ckbs_nonlinear. If the sub-problem solution is working well, this will be one. Much small step sizes indicate the sub-problem solution is not working well.

Penalty Parameter
The exact penalty function   $S( x_1 , \ldots , x_k ) + \alpha \sum_{k=1}^N \sum_{i=1}^\ell \max [ f_k ( x_k )_i , 0 ]$  is used to determine the line search step size. The value info(q, 7) is the penalty parameter   \alpha during iteration   q-1 of ckbs_nonlinear.

Example

Simple
The file get_started_ok.m contains a simple example and test of ckbs_nonlinear. It returns true if ckbs_nonlinear passes the tests and false otherwise.

Unconstrained
The option constraint = 'no_constraint' to the routine sine_wave_ok.m runs an unconstrained example / test.

Box Constraints
The option constraint = 'box_constraint' to the routine sine_wave_ok.m runs a example / test with upper and lower constraints.

Nonlinear Constraints
The option constraint = 'sine_constraint' to the routine sine_wave_ok.m runs a example / test with a nonlinear constraint.

Van Der Pol Oscillator
The file vanderpol_ok.m contains an example and test of ckbs_nonlinear estimating the position of a Van Der Pol oscillator (an oscillator with non-linear dynamics). It returns true if ckbs_nonlinear passes the tests and false otherwise.
Input File: src/ckbs_nonlinear.m