![]() |
Previous | Next |
[
x_out,
u_out,
info] = ckbs_nonlinear(
f_fun,
g_fun,
h_fun, ...
max_itr,
epsilon,
x_in,
z,
qinv,
rinv,
level)
\[
\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 ) ]^\T * R_k^{-1} * [ z_k - h_k ( x_k ) ]
\\
& + &
\frac{1}{2}
[ x_k - g_k ( x_{k-1} ) ]^\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.
\[
\begin{array}{rll}
{\rm minimize}
& S( x_1 , \ldots , x_N )
& {\rm w.r.t.} \; x_1 \in \R^n , \ldots , x_N \in \R^n
\\
{\rm subject \; to}
& f_k(x_k) \leq 0
& {\rm for} \; k = 1 , \ldots , N
\end{array}
\]
( 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^\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
.
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)
In the case where there are no constraints,
one can use the following function for f_fun:
[fk, Fk] = f_fun(k, xk)
n = size(xk, 1);
fk = -1;
Fk = zeros(1, n);
return
end
1 \leq k \leq N
.
n
. It specifies the state vector at time index k
\[
xk = x_k
\]
.
\ell
and
\[
fk = f_k ( x_k )
\]
\ell \times n
matrix given by
and
\[
Fk = \partial_k f_k ( x_k )
\]
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)
1 \leq k \leq N
.
The case
k = 1
serves to specify the initial state estimate.
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.
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.
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}
.
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)
1 \leq k \leq N
.
n
. It specifies the state vector at time index k
\[
xk = x_k
\]
.
m
and
\[
hk = h_k ( x_k )
\]
m \times n
matrix given by
and
\[
Hk = \partial_k h_k ( x_k )
\]
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
.
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
.
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
\]
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
.
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 == 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.
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
\]
\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
.
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(
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(
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)^\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 =
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(
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(
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(
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.
\[
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(
a, 7)
is the penalty parameter
\alpha
during iteration
q-1
of ckbs_nonlinear
.
ckbs_nonlinear
.
It returns true if ckbs_nonlinear
passes the tests
and false otherwise.
ckbs_nonlinear
with box constraints
and a nonlinear measurement function.
It returns true if ckbs_nonlinear
passes the tests
and false otherwise.
ckbs_nonlinear
with a sine wave constraint
and a nonlinear measurement function.
It returns true if ckbs_nonlinear
passes the tests
and false otherwise.