@@ -709,19 +709,27 @@ end
709709
710710function init_defectmat (
711711 model:: NonLinModel , estim:: StateEstimator{NT} , transcription:: CollocationMethod , Hp, Hc, _
712- )
713-
712+ ) where {NT<: Real }
713+ nx̂, nu, nd = estim. nx̂, model. nu, model. nd
714+ nZ = get_nZ (estim, transcription, Hp, Hc)
715+ Eŝ = zeros (NT, 0 , nZ)
716+ Gŝ = zeros (NT, 0 , nd)
717+ Jŝ = zeros (NT, 0 , nd* Hp)
718+ Kŝ = zeros (NT, 0 , nx̂)
719+ Vŝ = zeros (NT, 0 , nu)
720+ Bŝ = zeros (NT, 0 )
721+ return Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ
714722end
715723
716724"""
717725 init_defectmat(
718- model::NonLinModel, estim::IntenalModel {NT}, transcription::CollocationMethod, Hp, Hc, _
726+ model::NonLinModel, estim::InternalModel {NT}, transcription::CollocationMethod, Hp, Hc, _
719727 ) -> Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ
720728
721729Return empty matrices for [`InternalModel`](@ref) (state vector is not augmented).
722730"""
723731function init_defectmat (
724- model:: NonLinModel , estim:: IntenalModel {NT} , transcription:: CollocationMethod , Hp, Hc, _
732+ model:: NonLinModel , estim:: InternalModel {NT} , transcription:: CollocationMethod , Hp, Hc, _
725733) where {NT<: Real }
726734 nx̂, nu, nd = estim. nx̂, model. nu, model. nd
727735 nZ = get_nZ (estim, transcription, Hp, Hc)
@@ -778,7 +786,7 @@ The argument `nc` is the number of custom nonlinear inequality constraints in
778786finite numbers. `i_g` is a similar vector but for the indices of ``\m athbf{g}``. The method
779787also returns the ``\m athbf{A, A_{eq}}`` matrices and `neq` if `args` is provided. In such a
780788case, `args` needs to contain all the inequality and equality constraint matrices:
781- `A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_Wmin, A_Wmax, A_x̂min, A_x̂max, A_ŝ `.
789+ `A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_Wmin, A_Wmax, A_x̂min, A_x̂max, A_Ŝ `.
782790The integer `neq` is the number of nonlinear equality constraints in ``\m athbf{g_{eq}}``.
783791"""
784792function init_matconstraint_mpc (
@@ -795,7 +803,7 @@ function init_matconstraint_mpc(
795803 A_Ymin, A_Ymax,
796804 A_Wmin, A_Wmax,
797805 A_x̂min, A_x̂max,
798- A_ŝ
806+ A_Ŝ
799807 ) = args
800808 A = [
801809 A_Umin; A_Umax;
@@ -804,7 +812,7 @@ function init_matconstraint_mpc(
804812 A_Wmin; A_Wmax
805813 A_x̂min; A_x̂max;
806814 ]
807- Aeq = A_ŝ
815+ Aeq = A_Ŝ
808816 neq = 0
809817 end
810818 i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Ymin; i_Ymax; i_Wmin; i_Wmax; i_x̂min; i_x̂max]
@@ -821,9 +829,9 @@ function init_matconstraint_mpc(
821829 if isempty (args)
822830 A, Aeq, neq = nothing , nothing , nothing
823831 else
824- A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , A_Wmin, A_Wmax, _ , _ , A_ŝ = args
832+ A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , A_Wmin, A_Wmax, _ , _ , A_Ŝ = args
825833 A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Wmin; A_Wmax]
826- Aeq = A_ŝ
834+ Aeq = A_Ŝ
827835 neq = 0
828836 end
829837 i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Wmin; i_Wmax]
@@ -840,9 +848,9 @@ function init_matconstraint_mpc(
840848 if isempty (args)
841849 A, Aeq, neq = nothing , nothing , nothing
842850 else
843- A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , A_Wmin, A_Wmax, A_x̂min, A_x̂max, A_ŝ = args
851+ A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , A_Wmin, A_Wmax, A_x̂min, A_x̂max, A_Ŝ = args
844852 A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Wmin; A_Wmax; A_x̂min; A_x̂max]
845- Aeq = A_ŝ
853+ Aeq = A_Ŝ
846854 nΔŨ, nZ̃ = size (A_ΔŨmin)
847855 neq = nZ̃ - nΔŨ
848856 end
0 commit comments