Merged in add_prev_in_augmented_state (pull request #2)

added previous state in backward augmented state

Approved-by: Davide Cucci

roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugQprevX1.cppready

@@ -0,0 +1,44 @@
+double tmp0 = qos(3+_OFF);
+double tmp1 = x1(4+_OFF);
+double tmp2 = 0.01*tmp1;
+double tmp3 = 0. + tmp2;
+double tmp4 = qos(4+_OFF);
+double tmp5 = x1(5+_OFF);
+double tmp6 = qos(1+_OFF);
+double tmp7 = x1(6+_OFF);
+double tmp8 = -0.01*tmp7;
+double tmp9 = 0. + tmp8;
+double tmp10 = qos(2+_OFF);
+double tmp11 = x1(7+_OFF);
+double tmp12 = -0.01*tmp5;
+double tmp13 = 0. + tmp12;
+double tmp14 = -0.01*tmp11;
+double tmp15 = 0. + tmp14;
+double tmp16 = 0.01*tmp11;
+double tmp17 = 0. + tmp16;
+double tmp18 = 0.01*tmp5;
+double tmp19 = 0. + tmp18;
+double tmp20 = tmp6*tmp15;
+double tmp21 = -(tmp0*tmp3);
+double tmp22 = 0.01*tmp7;
+double tmp23 = 0. + tmp22;
+double tmp24 = -(tmp4*tmp3);
+double tmp25 = tmp10*tmp9;
+double tmp26 = tmp6*tmp3;
+double tmp27 = tmp6*tmp13;
+double tmp28 = tmp4*tmp13;
+double tmp29 = tmp6*tmp9;
+double tmp30 = -(tmp10*tmp3);
+double tmp31 = tmp0*tmp15;
+J(1+_OFF,4+_OFF) = tmp30 + tmp27 - tmp4*tmp9 - tmp0*tmp17;
+J(1+_OFF,5+_OFF) = tmp21 - tmp4*tmp19 + tmp29 - tmp10*tmp15;
+J(1+_OFF,6+_OFF) = tmp24 - tmp0*tmp13 - tmp10*tmp23 + tmp20;
+J(2+_OFF,4+_OFF) = tmp26 + tmp10*tmp13 - tmp0*tmp9 + tmp4*tmp17;
+J(2+_OFF,5+_OFF) = tmp4*tmp3 - tmp0*tmp19 + tmp25 + tmp20;
+J(2+_OFF,6+_OFF) = tmp21 + tmp28 + tmp6*tmp23 + tmp10*tmp15;
+J(3+_OFF,4+_OFF) = tmp24 + tmp0*tmp13 + tmp25 + tmp6*tmp17;
+J(3+_OFF,5+_OFF) = tmp26 + tmp10*tmp19 + tmp0*tmp9 - tmp4*tmp15;
+J(3+_OFF,6+_OFF) = tmp10*tmp3 + tmp27 - tmp4*tmp23 + tmp31;
+J(4+_OFF,4+_OFF) = tmp0*tmp3 + tmp28 + tmp29 - tmp10*tmp17;
+J(4+_OFF,5+_OFF) = tmp30 + tmp6*tmp19 + tmp4*tmp9 + tmp31;
+J(4+_OFF,6+_OFF) = tmp26 - tmp10*tmp13 + tmp0*tmp23 + tmp4*tmp15;

roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugQprevqOS.cppready

@@ -0,0 +1,44 @@
+double tmp0 = qos(3+_OFF);
+double tmp1 = -0.01*tmp0;
+double tmp2 = 0. + tmp1;
+double tmp3 = x1(4+_OFF);
+double tmp4 = qos(4+_OFF);
+double tmp5 = x1(5+_OFF);
+double tmp6 = qos(1+_OFF);
+double tmp7 = 0.01*tmp6;
+double tmp8 = 0. + tmp7;
+double tmp9 = x1(6+_OFF);
+double tmp10 = qos(2+_OFF);
+double tmp11 = x1(7+_OFF);
+double tmp12 = -0.01*tmp4;
+double tmp13 = 0. + tmp12;
+double tmp14 = -0.01*tmp10;
+double tmp15 = 0. + tmp14;
+double tmp16 = 0.01*tmp4;
+double tmp17 = 0. + tmp16;
+double tmp18 = tmp13*tmp3;
+double tmp19 = 0.01*tmp10;
+double tmp20 = 0. + tmp19;
+double tmp21 = -(tmp8*tmp11);
+double tmp22 = 0.01*tmp0;
+double tmp23 = 0. + tmp22;
+double tmp24 = tmp8*tmp3;
+double tmp25 = tmp2*tmp9;
+double tmp26 = tmp15*tmp3;
+double tmp27 = -(tmp8*tmp5);
+double tmp28 = tmp2*tmp3;
+double tmp29 = -(tmp8*tmp9);
+double tmp30 = tmp15*tmp5;
+double tmp31 = tmp13*tmp11;
+J(1+_OFF,1+_OFF) = tmp26 + tmp27 - tmp17*tmp9 - tmp2*tmp11;
+J(1+_OFF,2+_OFF) = tmp28 - tmp13*tmp5 + tmp29 - tmp20*tmp11;
+J(1+_OFF,3+_OFF) = tmp18 - tmp23*tmp5 - tmp15*tmp9 + tmp21;
+J(2+_OFF,1+_OFF) = tmp24 + tmp30 + tmp25 - tmp17*tmp11;
+J(2+_OFF,2+_OFF) = tmp18 + tmp2*tmp5 + tmp20*tmp9 + tmp21;
+J(2+_OFF,3+_OFF) = tmp23*tmp3 + tmp13*tmp5 + tmp8*tmp9 - tmp15*tmp11;
+J(3+_OFF,1+_OFF) = tmp17*tmp3 - tmp2*tmp5 + tmp15*tmp9 + tmp8*tmp11;
+J(3+_OFF,2+_OFF) = tmp24 - tmp20*tmp5 + tmp25 + tmp31;
+J(3+_OFF,3+_OFF) = tmp26 + tmp27 + tmp13*tmp9 + tmp23*tmp11;
+J(4+_OFF,1+_OFF) = tmp28 + tmp17*tmp5 + tmp29 + tmp15*tmp11;
+J(4+_OFF,2+_OFF) = tmp20*tmp3 + tmp8*tmp5 - tmp13*tmp9 + tmp2*tmp11;
+J(4+_OFF,3+_OFF) = tmp24 + tmp30 - tmp23*tmp9 + tmp31;

roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugVprevSO.cppready

@@ -0,0 +1,85 @@
+double tmp0 = 1/Dt12;
+double tmp1 = qos(1+_OFF);
+double tmp2 = 1.*tmp1;
+double tmp3 = 0. + tmp2;
+double tmp4 = qos(3+_OFF);
+double tmp5 = 1.*tmp4;
+double tmp6 = 0. + tmp5;
+double tmp7 = qos(2+_OFF);
+double tmp8 = 1.*tmp7;
+double tmp9 = 0. + tmp8;
+double tmp10 = qos(4+_OFF);
+double tmp11 = 1.*tmp10;
+double tmp12 = 0. + tmp11;
+double tmp13 = x1(6+_OFF);
+double tmp14 = x2(4+_OFF);
+double tmp15 = x1(7+_OFF);
+double tmp16 = x2(5+_OFF);
+double tmp17 = x1(4+_OFF);
+double tmp18 = x2(6+_OFF);
+double tmp19 = x1(5+_OFF);
+double tmp20 = x2(7+_OFF);
+double tmp21 = -(tmp3*tmp6);
+double tmp22 = tmp9*tmp12;
+double tmp23 = tmp21 + tmp22;
+double tmp24 = std::pow(tmp3,2);
+double tmp25 = std::pow(tmp9,2);
+double tmp26 = std::pow(tmp6,2);
+double tmp27 = -tmp26;
+double tmp28 = std::pow(tmp12,2);
+double tmp29 = -tmp28;
+double tmp30 = tmp24 + tmp25 + tmp27 + tmp29;
+double tmp31 = tmp9*tmp6;
+double tmp32 = tmp3*tmp12;
+double tmp33 = tmp31 + tmp32;
+double tmp34 = -2*tmp0*tmp15*tmp14;
+double tmp35 = 2*tmp0*tmp13*tmp16;
+double tmp36 = -2*tmp0*tmp19*tmp18;
+double tmp37 = 2*tmp0*tmp17*tmp20;
+double tmp38 = tmp34 + tmp35 + tmp36 + tmp37;
+double tmp39 = 2*tmp0*tmp13*tmp14;
+double tmp40 = 2*tmp0*tmp15*tmp16;
+double tmp41 = -2*tmp0*tmp17*tmp18;
+double tmp42 = -2*tmp0*tmp19*tmp20;
+double tmp43 = tmp39 + tmp40 + tmp41 + tmp42;
+double tmp44 = 2*tmp0*tmp15*tmp14;
+double tmp45 = -2*tmp0*tmp13*tmp16;
+double tmp46 = 2*tmp0*tmp19*tmp18;
+double tmp47 = -2*tmp0*tmp17*tmp20;
+double tmp48 = tmp44 + tmp45 + tmp46 + tmp47;
+double tmp49 = tmp3*tmp9;
+double tmp50 = tmp6*tmp12;
+double tmp51 = tmp49 + tmp50;
+double tmp52 = -2*tmp0*tmp19*tmp14;
+double tmp53 = 2*tmp0*tmp17*tmp16;
+double tmp54 = 2*tmp0*tmp15*tmp18;
+double tmp55 = -2*tmp0*tmp13*tmp20;
+double tmp56 = tmp52 + tmp53 + tmp54 + tmp55;
+double tmp57 = -(tmp3*tmp12);
+double tmp58 = tmp31 + tmp57;
+double tmp59 = -2*tmp0*tmp13*tmp14;
+double tmp60 = -2*tmp0*tmp15*tmp16;
+double tmp61 = 2*tmp0*tmp17*tmp18;
+double tmp62 = 2*tmp0*tmp19*tmp20;
+double tmp63 = tmp59 + tmp60 + tmp61 + tmp62;
+double tmp64 = -tmp25;
+double tmp65 = tmp24 + tmp64 + tmp26 + tmp29;
+double tmp66 = 2*tmp0*tmp19*tmp14;
+double tmp67 = -2*tmp0*tmp17*tmp16;
+double tmp68 = -2*tmp0*tmp15*tmp18;
+double tmp69 = 2*tmp0*tmp13*tmp20;
+double tmp70 = tmp66 + tmp67 + tmp68 + tmp69;
+double tmp71 = tmp24 + tmp64 + tmp27 + tmp28;
+double tmp72 = tmp3*tmp6;
+double tmp73 = tmp72 + tmp22;
+double tmp74 = -(tmp3*tmp9);
+double tmp75 = tmp74 + tmp50;
+J(1+_OFF,1+_OFF) = 2*tmp33*tmp38 + 2*tmp23*tmp43;
+J(1+_OFF,2+_OFF) = tmp30*tmp48 + 2*tmp23*tmp56;
+J(1+_OFF,3+_OFF) = tmp30*tmp63 + 2*tmp33*tmp70;
+J(2+_OFF,1+_OFF) = tmp65*tmp38 + 2*tmp51*tmp43;
+J(2+_OFF,2+_OFF) = 2*tmp58*tmp48 + 2*tmp51*tmp56;
+J(2+_OFF,3+_OFF) = 2*tmp58*tmp63 + tmp65*tmp70;
+J(3+_OFF,1+_OFF) = 2*tmp75*tmp38 + tmp71*tmp43;
+J(3+_OFF,2+_OFF) = 2*tmp73*tmp48 + tmp71*tmp56;
+J(3+_OFF,3+_OFF) = 2*tmp73*tmp63 + 2*tmp75*tmp70;

roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugVprevX0.cppready

@@ -0,0 +1,67 @@
+double tmp0 = 1/Dt01;
+double tmp1 = qos(1+_OFF);
+double tmp2 = qos(3+_OFF);
+double tmp3 = qos(2+_OFF);
+double tmp4 = qos(4+_OFF);
+double tmp5 = x1(4+_OFF);
+double tmp6 = x1(6+_OFF);
+double tmp7 = x1(5+_OFF);
+double tmp8 = x1(7+_OFF);
+double tmp9 = std::pow(tmp1,2);
+double tmp10 = std::pow(tmp3,2);
+double tmp11 = std::pow(tmp2,2);
+double tmp12 = -tmp11;
+double tmp13 = std::pow(tmp4,2);
+double tmp14 = -tmp13;
+double tmp15 = tmp9 + tmp10 + tmp12 + tmp14;
+double tmp16 = tmp7*tmp6;
+double tmp17 = -(tmp1*tmp2);
+double tmp18 = tmp3*tmp4;
+double tmp19 = tmp17 + tmp18;
+double tmp20 = tmp3*tmp2;
+double tmp21 = tmp1*tmp4;
+double tmp22 = tmp20 + tmp21;
+double tmp23 = std::pow(tmp8,2);
+double tmp24 = 2*tmp23;
+double tmp25 = std::pow(tmp7,2);
+double tmp26 = 2*tmp25;
+double tmp27 = std::pow(tmp6,2);
+double tmp28 = 2*tmp27;
+double tmp29 = tmp5*tmp6;
+double tmp30 = tmp5*tmp7;
+double tmp31 = -(tmp5*tmp8);
+double tmp32 = tmp16 + tmp31;
+double tmp33 = tmp7*tmp8;
+double tmp34 = tmp29 + tmp33;
+double tmp35 = -1 + tmp28 + tmp24;
+double tmp36 = -(tmp1*tmp4);
+double tmp37 = tmp20 + tmp36;
+double tmp38 = tmp5*tmp8;
+double tmp39 = tmp16 + tmp38;
+double tmp40 = tmp1*tmp3;
+double tmp41 = tmp2*tmp4;
+double tmp42 = tmp40 + tmp41;
+double tmp43 = -(tmp6*tmp8);
+double tmp44 = tmp30 + tmp43;
+double tmp45 = -tmp10;
+double tmp46 = tmp9 + tmp45 + tmp11 + tmp14;
+double tmp47 = -1 + tmp26 + tmp24;
+double tmp48 = -1 + tmp26 + tmp28;
+double tmp49 = -(tmp7*tmp8);
+double tmp50 = tmp29 + tmp49;
+double tmp51 = tmp6*tmp8;
+double tmp52 = tmp30 + tmp51;
+double tmp53 = tmp1*tmp2;
+double tmp54 = tmp53 + tmp18;
+double tmp55 = tmp9 + tmp45 + tmp12 + tmp13;
+double tmp56 = -(tmp1*tmp3);
+double tmp57 = tmp56 + tmp41;
+J(1+_OFF,1+_OFF) = -0.004*tmp0*tmp22*tmp32 - 0.004*tmp0*tmp19*tmp34 + 0.001*tmp0*tmp15*tmp35;
+J(1+_OFF,2+_OFF) = -0.002*tmp0*tmp15*tmp39 + 0.004*tmp0*tmp19*tmp44 + 0.002*tmp0*tmp22*tmp47;
+J(1+_OFF,3+_OFF) = 0.002*tmp0*tmp19*tmp48 + 0.002*tmp0*tmp15*tmp50 - 0.004*tmp0*tmp22*tmp52;
+J(2+_OFF,1+_OFF) = -0.002*tmp0*tmp46*tmp32 - 0.004*tmp0*tmp42*tmp34 + 0.002*tmp0*tmp37*tmp35;
+J(2+_OFF,2+_OFF) = -0.004*tmp0*tmp37*tmp39 + 0.004*tmp0*tmp42*tmp44 + 0.001*tmp0*tmp46*tmp47;
+J(2+_OFF,3+_OFF) = 0.002*tmp0*tmp42*tmp48 + 0.004*tmp0*tmp37*tmp50 - 0.002*tmp0*tmp46*tmp52;
+J(3+_OFF,1+_OFF) = -0.004*tmp0*tmp57*tmp32 - 0.002*tmp0*tmp55*tmp34 + 0.002*tmp0*tmp54*tmp35;
+J(3+_OFF,2+_OFF) = -0.004*tmp0*tmp54*tmp39 + 0.002*tmp0*tmp55*tmp44 + 0.002*tmp0*tmp57*tmp47;
+J(3+_OFF,3+_OFF) = 0.001*tmp0*tmp55*tmp48 + 0.004*tmp0*tmp54*tmp50 - 0.004*tmp0*tmp57*tmp52;