378
378
@test internalmodel2. nxs == 1
379
379
@test internalmodel2. nx̂ == 4
380
380
381
- f (x,u,d,_ ) = linmodel2 . A* x + linmodel2 . Bu* u + linmodel2 . Bd* d
382
- h (x,d,_ ) = linmodel2 . C* x + linmodel2 . Dd* d
383
- nonlinmodel = NonLinModel (f, h, Ts, 2 , 4 , 2 , 2 , solver= nothing )
381
+ f (x,u,d,model ) = model . A* x + model . Bu* u + model . Bd* d
382
+ h (x,d,model ) = model . C* x + model . Dd* d
383
+ nonlinmodel = NonLinModel (f, h, Ts, 2 , 4 , 2 , 2 , solver= nothing , p = linmodel2 )
384
384
internalmodel3 = InternalModel (nonlinmodel)
385
385
@test internalmodel3. nym == 2
386
386
@test internalmodel3. nyu == 0
499
499
@testitem " UnscentedKalmanFilter construction" setup= [SetupMPCtests] begin
500
500
using . SetupMPCtests, ControlSystemsBase, LinearAlgebra
501
501
linmodel1 = LinModel (sys,Ts,i_d= [3 ])
502
- f (x,u,d,_ ) = linmodel1 . A* x + linmodel1 . Bu* u + linmodel1 . Bd* d
503
- h (x,d,_ ) = linmodel1 . C* x + linmodel1 . Du* d
504
- nonlinmodel = NonLinModel (f, h, Ts, 2 , 4 , 2 , 1 , solver= nothing )
502
+ f (x,u,d,model ) = model . A* x + model . Bu* u + model . Bd* d
503
+ h (x,d,model ) = model . C* x + model . Du* d
504
+ nonlinmodel = NonLinModel (f, h, Ts, 2 , 4 , 2 , 1 , solver= nothing , p = linmodel1 )
505
505
506
506
ukf1 = UnscentedKalmanFilter (linmodel1)
507
507
@test ukf1. nym == 2
557
557
@testitem " UnscentedKalmanFilter estimator methods" setup= [SetupMPCtests] begin
558
558
using . SetupMPCtests, ControlSystemsBase, LinearAlgebra
559
559
linmodel1 = LinModel (sys,Ts,i_u= [1 ,2 ])
560
- f (x,u,_,_) = linmodel1. A* x + linmodel1. Bu* u
561
- h (x,_,_) = linmodel1. C* x
562
- nonlinmodel = setop! (NonLinModel (f, h, Ts, 2 , 2 , 2 , solver= nothing ), uop= [10 ,50 ], yop= [50 ,30 ])
560
+ f (x,u,_,model) = model. A* x + model. Bu* u
561
+ h (x,_,model) = model. C* x
562
+ nonlinmodel = NonLinModel (f, h, Ts, 2 , 2 , 2 , solver= nothing , p= linmodel1)
563
+ nonlinmodel = setop! (nonlinmodel, uop= [10 ,50 ], yop= [50 ,30 ])
563
564
ukf1 = UnscentedKalmanFilter (nonlinmodel)
564
565
preparestate! (ukf1, [50 , 30 ])
565
566
@test updatestate! (ukf1, [10 , 50 ], [50 , 30 ]) ≈ zeros (4 ) atol= 1e-9
630
631
setmodel! (ukf1, Q̂= [1e-3 ], R̂= [1e-6 ])
631
632
@test ukf1. Q̂ ≈ [1e-3 ]
632
633
@test ukf1. R̂ ≈ [1e-6 ]
633
- f (x,u,d,_ ) = linmodel . A* x + linmodel . Bu* u + linmodel . Bd* d
634
- h (x,d,_ ) = linmodel . C* x + linmodel . Du* d
635
- nonlinmodel = NonLinModel (f, h, 10.0 , 1 , 1 , 1 )
634
+ f (x,u,d,model ) = model . A* x + model . Bu* u + model . Bd* d
635
+ h (x,d,model ) = model . C* x + model . Du* d
636
+ nonlinmodel = NonLinModel (f, h, 10.0 , 1 , 1 , 1 , solver = nothing , p = linmodel )
636
637
ukf2 = UnscentedKalmanFilter (nonlinmodel, nint_ym= 0 )
637
638
setmodel! (ukf2, Q̂= [1e-3 ], R̂= [1e-6 ])
638
639
@test ukf2. Q̂ ≈ [1e-3 ]
@@ -642,10 +643,12 @@ end
642
643
643
644
@testitem " ExtendedKalmanFilter construction" setup= [SetupMPCtests] begin
644
645
using . SetupMPCtests, ControlSystemsBase, LinearAlgebra
646
+ using DifferentiationInterface
647
+ import FiniteDiff
645
648
linmodel1 = LinModel (sys,Ts,i_d= [3 ])
646
- f (x,u,d,_ ) = linmodel1 . A* x + linmodel1 . Bu* u + linmodel1 . Bd* d
647
- h (x,d,_ ) = linmodel1 . C* x + linmodel1 . Du* d
648
- nonlinmodel = NonLinModel (f, h, Ts, 2 , 4 , 2 , 1 , solver= nothing )
649
+ f (x,u,d,model ) = model . A* x + model . Bu* u + model . Bd* d
650
+ h (x,d,model ) = model . C* x + model . Du* d
651
+ nonlinmodel = NonLinModel (f, h, Ts, 2 , 4 , 2 , 1 , solver= nothing , p = linmodel1 )
649
652
650
653
ekf1 = ExtendedKalmanFilter (linmodel1)
651
654
@test ekf1. nym == 2
@@ -689,17 +692,23 @@ end
689
692
@test ekf8. Q̂ ≈ I (6 )
690
693
@test ekf8. R̂ ≈ I (2 )
691
694
695
+ ekf9 = ExtendedKalmanFilter (nonlinmodel, jacobian= AutoFiniteDiff ())
696
+ @test ekf9. jacobian === AutoFiniteDiff ()
697
+
692
698
linmodel2 = LinModel {Float32} (0.5 * ones (1 ,1 ), ones (1 ,1 ), ones (1 ,1 ), zeros (1 ,0 ), zeros (1 ,0 ), 1.0 )
693
699
ekf8 = ExtendedKalmanFilter (linmodel2)
694
700
@test isa (ekf8, ExtendedKalmanFilter{Float32})
695
701
end
696
702
697
703
@testitem " ExtendedKalmanFilter estimator methods" setup= [SetupMPCtests] begin
698
704
using . SetupMPCtests, ControlSystemsBase, LinearAlgebra
705
+ using DifferentiationInterface
706
+ import FiniteDiff
699
707
linmodel1 = LinModel (sys,Ts,i_u= [1 ,2 ])
700
- f (x,u,_,_) = linmodel1. A* x + linmodel1. Bu* u
701
- h (x,_,_) = linmodel1. C* x
702
- nonlinmodel = setop! (NonLinModel (f, h, Ts, 2 , 2 , 2 , solver= nothing ), uop= [10 ,50 ], yop= [50 ,30 ])
708
+ f (x,u,_,model) = model. A* x + model. Bu* u
709
+ h (x,_,model) = model. C* x
710
+ nonlinmodel = NonLinModel (f, h, Ts, 2 , 2 , 2 , solver= nothing , p= linmodel1)
711
+ nonlinmodel = setop! (nonlinmodel, uop= [10 ,50 ], yop= [50 ,30 ])
703
712
ekf1 = ExtendedKalmanFilter (nonlinmodel)
704
713
preparestate! (ekf1, [50 , 30 ])
705
714
@test updatestate! (ekf1, [10 , 50 ], [50 , 30 ]) ≈ zeros (4 ) atol= 1e-9
741
750
x̂ = updatestate! (ekf3, [0 ], [0 ])
742
751
@test x̂ ≈ [0 , 0 ]
743
752
@test isa (x̂, Vector{Float32})
753
+ ekf4 = ExtendedKalmanFilter (nonlinmodel, jacobian= AutoFiniteDiff ())
754
+ preparestate! (ekf4, [50 , 30 ])
755
+ @test updatestate! (ekf4, [10 , 50 ], [50 , 30 ]) ≈ zeros (4 ) atol= 1e-9
756
+ preparestate! (ekf4, [50 , 30 ])
757
+ @test evaloutput (ekf4) ≈ ekf4 () ≈ [50 , 30 ]
744
758
end
745
759
746
760
@testitem " ExtendedKalmanFilter set model" setup= [SetupMPCtests] begin
@@ -1008,9 +1022,10 @@ end
1008
1022
@testitem " MovingHorizonEstimator fallbacks for arrival covariance estimation" setup= [SetupMPCtests] begin
1009
1023
using . SetupMPCtests, ControlSystemsBase, LinearAlgebra
1010
1024
linmodel = setop! (LinModel (sys,Ts,i_u= [1 ,2 ], i_d= [3 ]), uop= [10 ,50 ], yop= [50 ,30 ], dop= [5 ])
1011
- f (x,u,d,_) = linmodel. A* x + linmodel. Bu* u + linmodel. Bd* d
1012
- h (x,d,_) = linmodel. C* x + linmodel. Dd* d
1013
- nonlinmodel = setop! (NonLinModel (f, h, Ts, 2 , 4 , 2 , 1 , solver= nothing ), uop= [10 ,50 ], yop= [50 ,30 ], dop= [5 ])
1025
+ f (x,u,d,model) = model. A* x + model. Bu* u + model. Bd* d
1026
+ h (x,d,model) = model. C* x + model. Dd* d
1027
+ nonlinmodel = NonLinModel (f, h, Ts, 2 , 4 , 2 , 1 , p= linmodel, solver= nothing )
1028
+ nonlinmodel = setop! (nonlinmodel, uop= [10 ,50 ], yop= [50 ,30 ], dop= [5 ])
1014
1029
mhe = MovingHorizonEstimator (nonlinmodel, nint_ym= 0 , He= 1 )
1015
1030
preparestate! (mhe, [50 , 30 ], [5 ])
1016
1031
updatestate! (mhe, [10 , 50 ], [50 , 30 ], [5 ])
@@ -1085,9 +1100,10 @@ end
1085
1100
setconstraint! (mhe2, C_v̂min= 0.05 (31 : 38 ), C_v̂max= 0.06 (31 : 38 ))
1086
1101
@test all ((- mhe2. con. A_V̂min[:, end ], - mhe2. con. A_V̂max[:, end ]) .≈ (0.05 (31 : 38 ), 0.06 (31 : 38 )))
1087
1102
1088
- f (x,u,d,_) = linmodel1. A* x + linmodel1. Bu* u
1089
- h (x,d,_) = linmodel1. C* x
1090
- nonlinmodel = setop! (NonLinModel (f, h, Ts, 2 , 2 , 2 , solver= nothing ), uop= [10 ,50 ], yop= [50 ,30 ])
1103
+ f (x,u,d,model) = model. A* x + model. Bu* u
1104
+ h (x,d,model) = model. C* x
1105
+ nonlinmodel = NonLinModel (f, h, Ts, 2 , 2 , 2 , p= linmodel1, solver= nothing )
1106
+ nonlinmodel = setop! (nonlinmodel, uop= [10 ,50 ], yop= [50 ,30 ])
1091
1107
1092
1108
mhe3 = MovingHorizonEstimator (nonlinmodel, He= 4 , nint_ym= 0 , Cwt= 1e3 )
1093
1109
setconstraint! (mhe3, C_x̂min= 0.01 (1 : 10 ), C_x̂max= 0.02 (1 : 10 ))
@@ -1181,9 +1197,10 @@ end
1181
1197
info = getinfo (mhe)
1182
1198
@test info[:V̂ ] ≈ [- 1 ,- 1 ] atol= 5e-2
1183
1199
1184
- f (x,u,_,_) = linmodel1. A* x + linmodel1. Bu* u
1185
- h (x,_,_) = linmodel1. C* x
1186
- nonlinmodel = setop! (NonLinModel (f, h, Ts, 2 , 2 , 2 , solver= nothing ), uop= [10 ,50 ], yop= [50 ,30 ])
1200
+ f (x,u,_,model) = model. A* x + model. Bu* u
1201
+ h (x,_,model) = model. C* x
1202
+ nonlinmodel = NonLinModel (f, h, Ts, 2 , 2 , 2 , p= linmodel1, solver= nothing )
1203
+ nonlinmodel = setop! (nonlinmodel, uop= [10 ,50 ], yop= [50 ,30 ])
1187
1204
mhe2 = MovingHorizonEstimator (nonlinmodel, He= 1 , nint_ym= 0 )
1188
1205
1189
1206
setconstraint! (mhe2, x̂min= [- 100 ,- 100 ], x̂max= [100 ,100 ])
@@ -1266,9 +1283,9 @@ end
1266
1283
setmodel! (mhe, Q̂= [1e-3 ], R̂= [1e-6 ])
1267
1284
@test mhe. Q̂ ≈ [1e-3 ]
1268
1285
@test mhe. R̂ ≈ [1e-6 ]
1269
- f (x,u,d,_ ) = linmodel . A* x + linmodel . Bu* u + linmodel . Bd* d
1270
- h (x,d,_ ) = linmodel . C* x + linmodel . Du* d
1271
- nonlinmodel = NonLinModel (f, h, 10.0 , 1 , 1 , 1 )
1286
+ f (x,u,d,model ) = model . A* x + model . Bu* u + model . Bd* d
1287
+ h (x,d,model ) = model . C* x + model . Du* d
1288
+ nonlinmodel = NonLinModel (f, h, 10.0 , 1 , 1 , 1 , p = linmodel, solver = nothing )
1272
1289
mhe2 = MovingHorizonEstimator (nonlinmodel, He= 1 , nint_ym= 0 )
1273
1290
setmodel! (mhe2, Q̂= [1e-3 ], R̂= [1e-6 ])
1274
1291
@test mhe2. Q̂ ≈ [1e-3 ]
@@ -1307,9 +1324,9 @@ end
1307
1324
updatestate! (kf, [11 , 50 ], y, [25 ])
1308
1325
end
1309
1326
@test X̂_mhe ≈ X̂_kf atol= 1e-3 rtol= 1e-3
1310
- f = (x,u,d,_ ) -> linmodel1 . A* x + linmodel1 . Bu* u + linmodel1 . Bd* d
1311
- h = (x,d,_ ) -> linmodel1 . C* x + linmodel1 . Dd* d
1312
- nonlinmodel = NonLinModel (f, h, Ts, 2 , 4 , 2 , 1 , solver= nothing )
1327
+ f = (x,u,d,model ) -> model . A* x + model . Bu* u + model . Bd* d
1328
+ h = (x,d,model ) -> model . C* x + model . Dd* d
1329
+ nonlinmodel = NonLinModel (f, h, Ts, 2 , 4 , 2 , 1 , p = linmodel1, solver= nothing )
1313
1330
nonlinmodel = setop! (nonlinmodel, uop= [10 ,50 ], yop= [50 ,30 ], dop= [20 ])
1314
1331
mhe = MovingHorizonEstimator (nonlinmodel, He= 5 , nint_ym= 0 , direct= false )
1315
1332
ukf = UnscentedKalmanFilter (nonlinmodel, nint_ym= 0 , direct= false )
@@ -1356,9 +1373,9 @@ end
1356
1373
@testitem " MovingHorizonEstimator LinModel v.s. NonLinModel" setup= [SetupMPCtests] begin
1357
1374
using . SetupMPCtests, ControlSystemsBase, LinearAlgebra, JuMP, Ipopt
1358
1375
linmodel = setop! (LinModel (sys,Ts,i_d= [3 ]), uop= [10 ,50 ], yop= [50 ,30 ], dop= [20 ])
1359
- f = (x,u,d,_ ) -> linmodel . A* x + linmodel . Bu* u + linmodel . Bd* d
1360
- h = (x,d,_ ) -> linmodel . C* x + linmodel . Dd* d
1361
- nonlinmodel = NonLinModel (f, h, Ts, 2 , 4 , 2 , 1 , solver= nothing )
1376
+ f = (x,u,d,model ) -> model . A* x + model . Bu* u + model . Bd* d
1377
+ h = (x,d,model ) -> model . C* x + model . Dd* d
1378
+ nonlinmodel = NonLinModel (f, h, Ts, 2 , 4 , 2 , 1 , p = linmodel, solver= nothing )
1362
1379
nonlinmodel = setop! (nonlinmodel, uop= [10 ,50 ], yop= [50 ,30 ], dop= [20 ])
1363
1380
optim = JuMP. Model (optimizer_with_attributes (Ipopt. Optimizer, " sb" => " yes" ))
1364
1381
mhe_lin = MovingHorizonEstimator (linmodel, He= 5 , nint_ym= 0 , direct= true , optim= optim)
0 commit comments