The Nonlinear Constrained Kalman-Bucy Smoother

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

. 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

. 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

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

. It specifies the state vector at time index k


 \[

      xk = x_k

\]

.

hk
The h_fun result hk is an column vector with length

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

.

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 .

max_grad
The value



      max_grad(q) = info(q, 2)

is a bound on the partial of the Lagrangian with respect to

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