src/Matriplex/GenMPlexOps.pl

0001 #!/usr/bin/perl
0002
0003 use lib "../Matriplex";
0004
0005 use GenMul;
0006 use warnings;
0007
0008 #------------------------------------------------------------------------------
0009 ### simple general 3x3 matrix times 3 vector multiplication for CF MPlex
0010
0011 $A = new GenMul::Matrix('name'=>'a', 'M'=>3, 'N'=>3);
0012
0013 $B = new GenMul::Matrix('name'=>'b', 'M'=>3, 'N'=>1);
0014
0015 $C = new GenMul::Matrix('name'=>'c', 'M'=>3, 'N'=>1);
0016
0017 $m = new GenMul::Multiply;
0018
0019 $m->dump_multiply_std_and_intrinsic("CFMatrix33Vector3.ah",
0020                                     $A, $B, $C);
0021
0022 #------------------------------------------------------------------------------
0023 ###updateParametersMPlex -- propagated errors in CCS coordinates
0024 # propErr_ccs = jac_ccs * propErr * jac_ccsT
0025
0026 $jac_ccs = new GenMul::Matrix('name'=>'a', 'M'=>6, 'N'=>6);
0027 $jac_ccs->set_pattern(<<"FNORD");
0028 1 0 0 0 0 0
0029 0 1 0 0 0 0
0030 0 0 1 0 0 0
0031 0 0 0 x x 0
0032 0 0 0 x x 0
0033 0 0 0 x x x
0034 FNORD
0035
0036 $propErr = new GenMul::MatrixSym('name'=>'b', 'M'=>6, 'N'=>6);
0037
0038 $temp   = new GenMul::Matrix('name'=>'c', 'M'=>6, 'N'=>6);
0039
0040 $m = new GenMul::Multiply;
0041
0042 $m->dump_multiply_std_and_intrinsic("CCSErr.ah",
0043                                     $jac_ccs, $propErr, $temp);
0044
0045 $jac_ccsT = new GenMul::MatrixTranspose($jac_ccs);
0046 $propErr_ccs = new GenMul::MatrixSym('name'=>'c', 'M'=>6, 'N'=>6);
0047 $temp  ->{name} = 'b';
0048
0049 $m->dump_multiply_std_and_intrinsic("CCSErrTransp.ah",
0050                                     $temp, $jac_ccsT, $propErr_ccs);
0051
0052 #------------------------------------------------------------------------------
0053 ###updateParametersMPlex -- updated errors in cartesian coordinates
0054 # outErr = jac_back_ccs * outErr_ccs * jac_back_ccsT
0055
0056 $jac_back_ccs = new GenMul::Matrix('name'=>'a', 'M'=>6, 'N'=>6);
0057 $jac_back_ccs->set_pattern(<<"FNORD");
0058 1 0 0 0 0 0
0059 0 1 0 0 0 0
0060 0 0 1 0 0 0
0061 0 0 0 x x 0
0062 0 0 0 x x 0
0063 0 0 0 x 0 x
0064 FNORD
0065
0066 $outErr_ccs = new GenMul::MatrixSym('name'=>'b', 'M'=>6, 'N'=>6);
0067
0068 $temp   = new GenMul::Matrix('name'=>'c', 'M'=>6, 'N'=>6);
0069
0070 $m = new GenMul::Multiply;
0071
0072 $m->dump_multiply_std_and_intrinsic("CartesianErr.ah",
0073                                     $jac_back_ccs, $outErr_ccs, $temp);
0074
0075 $jac_back_ccsT = new GenMul::MatrixTranspose($jac_back_ccs);
0076 $outErr = new GenMul::MatrixSym('name'=>'c', 'M'=>6, 'N'=>6);
0077 $temp  ->{name} = 'b';
0078
0079 $m->dump_multiply_std_and_intrinsic("CartesianErrTransp.ah",
0080                                     $temp, $jac_back_ccsT, $outErr);
0081
0082 #------------------------------------------------------------------------------
0083 ###updateParametersMPlex -- first term to get kalman gain (H^T*G)
0084 # temp = rot * resErr_loc
0085
0086 $rot = new GenMul::Matrix('name'=>'a', 'M'=>3, 'N'=>3);
0087 $rot->set_pattern(<<"FNORD");
0088 x 0 x
0089 x 0 x
0090 0 1 0
0091 FNORD
0092
0093 $resErr_loc = new GenMul::MatrixSym('name'=>'b', 'M'=>3, 'N'=>3);
0094
0095 $temp   = new GenMul::Matrix('name'=>'c', 'M'=>3, 'N'=>3);
0096
0097 $m = new GenMul::Multiply;
0098
0099 $m->dump_multiply_std_and_intrinsic("KalmanHTG.ah",
0100                                     $rot, $resErr_loc, $temp);
0101
0102
0103 #------------------------------------------------------------------------------
0104 ###updateParametersMPlex -- kalman gain
0105 # K = propErr_ccs * resErrTmpLH
0106
0107 $propErr_ccs = new GenMul::MatrixSym('name'=>'a', 'M'=>6, 'N'=>6);
0108
0109 $resErrTmpLH  = new GenMul::Matrix('name'=>'b', 'M'=>6, 'N'=>3);
0110 $resErrTmpLH->set_pattern(<<"FNORD");
0111 x x 0
0112 x x 0
0113 x x 0
0114 0 0 0
0115 0 0 0
0116 0 0 0
0117 FNORD
0118
0119 $K   = new GenMul::Matrix('name'=>'c', 'M'=>6, 'N'=>3);
0120
0121 $m = new GenMul::Multiply;
0122
0123 $m->dump_multiply_std_and_intrinsic("KalmanGain.ah",
0124                                     $propErr_ccs, $resErrTmpLH, $K);
0125 #------------------------------------------------------------------------------
0126 ###updateParametersMPlex -- kalman gain
0127 # K = propErr * resErr2x2
0128
0129 $propErr = new GenMul::MatrixSym('name'=>'a', 'M'=>6, 'N'=>6);
0130
0131 $resErr2x2  = new GenMul::MatrixSym('name'=>'b', 'M'=>2, 'N'=>2);
0132
0133 $K   = new GenMul::Matrix('name'=>'c', 'M'=>6, 'N'=>2);
0134
0135 {
0136   my $m_kg = new GenMul::Multiply('no_size_check' => 1);
0137
0138   $m_kg->dump_multiply_std_and_intrinsic("KalmanGain62.ah",
0139                      $propErr, $resErr2x2, $K);
0140 }
0141
0142 #------------------------------------------------------------------------------
0143 ###updateParametersMPlex -- KH
0144 # KH = K * H
0145
0146 $K   = new GenMul::Matrix('name'=>'a', 'M'=>6, 'N'=>3);
0147 $K->set_pattern(<<"FNORD");
0148 x x 0
0149 x x 0
0150 x x 0
0151 x x 0
0152 x x 0
0153 x x 0
0154 FNORD
0155
0156 $H   = new GenMul::Matrix('name'=>'b', 'M'=>3, 'N'=>6);
0157 $H->set_pattern(<<"FNORD");
0158 x x 0 0 0 0
0159 0 0 1 0 0 0
0160 x x 0 0 0 0
0161 FNORD
0162
0163 $KH   = new GenMul::Matrix('name'=>'c', 'M'=>6, 'N'=>6);
0164
0165 $m = new GenMul::Multiply;
0166
0167 $m->dump_multiply_std_and_intrinsic("KH.ah",
0168                                     $K, $H, $KH);
0169
0170 #------------------------------------------------------------------------------
0171 ###updateParametersMPlex -- KH * C
0172 # temp = KH * propErr_ccs
0173
0174 $KH   = new GenMul::Matrix('name'=>'a', 'M'=>6, 'N'=>6);
0175 $KH->set_pattern(<<"FNORD");
0176 x x x 0 0 0
0177 x x x 0 0 0
0178 x x x 0 0 0
0179 x x x 0 0 0
0180 x x x 0 0 0
0181 x x x 0 0 0
0182 FNORD
0183
0184 $propErr_ccs = new GenMul::MatrixSym('name'=>'b', 'M'=>6, 'N'=>6);
0185
0186 $temp   = new GenMul::MatrixSym('name'=>'c', 'M'=>6, 'N'=>6);
0187
0188 $m = new GenMul::Multiply;
0189
0190 $m->dump_multiply_std_and_intrinsic("KHC.ah",
0191                                     $KH, $propErr_ccs, $temp);
0192
0193 #------------------------------------------------------------------------------
0194
0195 ###updateParametersMPlex -- KH * C with KH=K dim 6x2
0196 # temp = KH * propErr
0197
0198 $KH   = new GenMul::Matrix('name'=>'a', 'M'=>6, 'N'=>2);
0199 $KH->set_pattern(<<"FNORD");
0200 x x
0201 x x
0202 x x
0203 x x
0204 x x
0205 x x
0206 FNORD
0207
0208 $propErr = new GenMul::MatrixSym('name'=>'b', 'M'=>6, 'N'=>6);
0209
0210 $temp   = new GenMul::MatrixSym('name'=>'c', 'M'=>6, 'N'=>6);
0211
0212 {
0213   my $m_kg = new GenMul::Multiply('no_size_check' => 1);
0214
0215   $m_kg->dump_multiply_std_and_intrinsic("K62HC.ah",
0216                       $KH, $propErr, $temp);
0217 }
0218
0219 #------------------------------------------------------------------------------
0220
0221 ### computeChi2MPlex -- similarity to rotate errors, two ops.
0222 # resErr_loc = rotT * resErr_glo * rotTT
0223
0224 $rotT = new GenMul::Matrix('name'=>'a', 'M'=>3, 'N'=>3);
0225 $rotT->set_pattern(<<"FNORD");
0226 x x 0
0227 0 0 1
0228 x x 0
0229 FNORD
0230
0231 $resErr_glo = new GenMul::MatrixSym('name'=>'b', 'M'=>3, 'N'=>3);
0232
0233 $temp   = new GenMul::Matrix('name'=>'c', 'M'=>3, 'N'=>3);
0234
0235 $m = new GenMul::Multiply;
0236
0237 $m->dump_multiply_std_and_intrinsic("ProjectResErr.ah",
0238                                     $rotT, $resErr_glo, $temp);
0239
0240 $roTT = new GenMul::MatrixTranspose($rotT);
0241 $resErr_loc = new GenMul::MatrixSym('name'=>'c', 'M'=>3, 'N'=>3);
0242 $temp  ->{name} = 'b';
0243
0244 $m->dump_multiply_std_and_intrinsic("ProjectResErrTransp.ah",
0245                                     $temp, $roTT, $resErr_loc);
0246
0247 #------------------------------------------------------------------------------
0248
0249 ### Propagate Helix To R -- final similarity, two ops.
0250
0251 # outErr = errProp * outErr * errPropT
0252 #   outErr is symmetric
0253
0254 my $DIM = 6;
0255
0256 $errProp = new GenMul::Matrix('name'=>'a', 'M'=>$DIM, 'N'=>$DIM);
0257 $errProp->set_pattern(<<"FNORD");
0258 x x 0 x x 0
0259 x x 0 x x 0
0260 x x 1 x x x
0261 x x 0 x x 0
0262 x x 0 x x 0
0263 0 0 0 0 0 1
0264 FNORD
0265 #switch to the one below when moving to CCS coordinates only
0266 #x x 0 x x 0
0267 #x x 0 x x 0
0268 #x x 1 x x x
0269 #0 0 0 1 0 0
0270 #x x 0 x x 0
0271 #0 0 0 0 0 1
0272 #FNORD
0273
0274 $outErr = new GenMul::MatrixSym('name'=>'b', 'M'=>$DIM, 'N'=>$DIM);
0275
0276 $temp   = new GenMul::Matrix('name'=>'c', 'M'=>$DIM, 'N'=>$DIM);
0277
0278
0279 $errPropT = new GenMul::MatrixTranspose($errProp);
0280 $errPropT->print_info();
0281 $errPropT->print_pattern();
0282
0283 # ----------------------------------------------------------------------
0284
0285 $m = new GenMul::Multiply;
0286
0287 # outErr and c are just templates ...
0288
0289 $m->dump_multiply_std_and_intrinsic("MultHelixProp.ah",
0290                                     $errProp, $outErr, $temp);
0291
0292 $temp  ->{name} = 'b';
0293 $outErr->{name} = 'c';
0294
0295 ### XXX fix this ... in accordance with what is in Propagation.cc
0296 $m->dump_multiply_std_and_intrinsic("MultHelixPropTransp.ah",
0297                                     $temp, $errPropT, $outErr);
0298
0299 #######################################
0300 ###          ENDCAP version         ###
0301 #######################################
0302
0303 $errProp->set_pattern(<<"FNORD");
0304 1 0 x x x x
0305 0 1 x x x x
0306 0 0 0 0 0 0
0307 0 0 0 1 0 0
0308 0 0 x x 1 x
0309 0 0 0 0 0 1
0310 FNORD
0311
0312 $temp  ->{name} = 'c';
0313 $outErr->{name} = 'b';
0314
0315 $errPropT = new GenMul::MatrixTranspose($errProp);
0316 $m->dump_multiply_std_and_intrinsic("MultHelixPropEndcap.ah",
0317                                     $errProp, $outErr, $temp);
0318
0319 $temp  ->{name} = 'b';
0320 $outErr->{name} = 'c';
0321
0322 ### XXX fix this ... in accordance with what is in Propagation.cc
0323 $m->dump_multiply_std_and_intrinsic("MultHelixPropTranspEndcap.ah",
0324                                     $temp, $errPropT, $outErr);
0325
0326
0327 ##############################
0328 ### updateParameters       ###
0329 ##############################
0330
0331 #declared first on its own because propErr sees many uses
0332 my $propErr_M = 6;
0333 $propErr = new GenMul::MatrixSym('name' => 'a',
0334                                  'M'    => $propErr_M); #will have to remember to re'name' it based on location in function
0335
0336 my $propErrT_M = 6;
0337 $propErrT = new GenMul::MatrixTranspose($propErr); #will have to remember to re'name' it based on location in function
0338
0339
0340
0341 ### kalmanGain =  = propErr * (projMatrixT * resErrInv)
0342 $resErrInv = new GenMul::MatrixSym('name'=>'b', 'M'=>3, 'N'=>3);
0343
0344 $kalmanGain = new GenMul::Matrix('name'=>'c', 'M' => 6, 'N' => 3);
0345
0346 {
0347   my $m_kg = new GenMul::Multiply('no_size_check' => 1);
0348
0349   $m_kg->dump_multiply_std_and_intrinsic("upParam_MultKalmanGain.ah",
0350                                          $propErr, $resErrInv, $kalmanGain);
0351 }
0352
0353
0354 ### updatedErrs = propErr - propErr^T * simil * propErr
0355 # Going to skip the subtraction for now
0356 my $simil_M = 6;
0357 $simil = new GenMul::MatrixSym('name'=>'a', 'M'=>$simil_M);
0358 $simil->set_pattern(<<"FNORD");
0359 x
0360 x x
0361 x x x
0362 0 0 0 0
0363 0 0 0 0 0
0364 0 0 0 0 0 0
0365 FNORD
0366
0367 $propErr->{name} = 'b';
0368
0369 my $temp_simil_x_propErr_M = 6;
0370 my $temp_simil_x_propErr_N = 6;
0371 $temp_simil_x_propErr = new GenMul::Matrix('name'=>'c',
0372                                            'M'=>$temp_simil_x_propErr_M,
0373                                            'N'=>$temp_simil_x_propErr_N);
0374
0375 $m->dump_multiply_std_and_intrinsic("upParam_simil_x_propErr.ah",
0376                                     $simil, $propErr, $temp_simil_x_propErr);
0377
0378 $temp_simil_x_propErr->{name} = 'b';
0379 $temp_simil_x_propErr->set_pattern(<<"FNORD");
0380 x x x x x x
0381 x x x x x x
0382 x x x x x x
0383 0 0 0 0 0 0
0384 0 0 0 0 0 0
0385 0 0 0 0 0 0
0386 FNORD
0387
0388 #? This one is symmetric but the output can't handle it... need to fix
0389 #$temp_propErrT_x_simil_propErr = new GenMul::MatrixSym('name'=>'c', 'M'=>$propErrT_M, 'N'=>$temp_simil_x_propErr_N);
0390
0391
0392 $temp_propErrT_x_simil_propErr = new GenMul::MatrixSym('name'=>'c', 'M'=>$propErrT_M);
0393
0394 $m->dump_multiply_std_and_intrinsic("upParam_propErrT_x_simil_propErr.ah",
0395                                     $propErrT, $temp_simil_x_propErr, $temp_propErrT_x_simil_propErr);
0396
0397
0398 {
0399   my $temp = new GenMul::MatrixSym('name' => 'c', 'M' => 6);
0400
0401   my $m_kg = new GenMul::Multiply('no_size_check' => 1);
0402
0403   $kalmanGain->{name} = 'a';
0404
0405   $m_kg->dump_multiply_std_and_intrinsic("upParam_kalmanGain_x_propErr.ah",
0406                                          $kalmanGain, $propErr, $temp);
0407 }