ekf2_derivation: create functions to generate innov_var or HK only

This is required when the innovation variance computation and the fusion are performed in two different functions.
2026-05-23 06:36:45 +08:00 · 2022-07-01 15:26:23 +02:00
parent 6225fae1d6
commit 503aa87957
10 changed files with 184 additions and 1 deletions
@@ -62,6 +62,12 @@ Kfusion(22) = HKX21*(HKX10*P(0,22) - HKX11*P(2,22) + HKX12*P(3,22) + HKX4*P(16,2
 Kfusion(23) = HKX21*(HKX10*P(0,23) - HKX11*P(2,23) + HKX12*P(3,23) + HKX4*P(16,23) + HKX7*P(17,23) - HKX8*P(18,23) + HKX9*P(1,23) + P(19,23));


+// Predicted observation
+
+
+// Innovation variance
+
+
 // Axis 1 equations
 // Sub Expressions
 const float HKY0 = magD*q1 + magE*q0 - magN*q3;
@@ -126,6 +132,12 @@ Kfusion(22) = HKY21*(HKY10*P(0,22) + HKY11*P(1,22) - HKY12*P(3,22) + HKY5*P(17,2
 Kfusion(23) = HKY21*(HKY10*P(0,23) + HKY11*P(1,23) - HKY12*P(3,23) + HKY5*P(17,23) + HKY7*P(18,23) - HKY8*P(16,23) + HKY9*P(2,23) + P(20,23));


+// Predicted observation
+
+
+// Innovation variance
+
+
 // Axis 2 equations
 // Sub Expressions
 const float HKZ0 = magD*q0 - magE*q1 + magN*q2;
@@ -190,3 +202,9 @@ Kfusion(22) = HKZ21*(HKZ10*P(0,22) - HKZ11*P(1,22) + HKZ12*P(2,22) + HKZ6*P(18,2
 Kfusion(23) = HKZ21*(HKZ10*P(0,23) - HKZ11*P(1,23) + HKZ12*P(2,23) + HKZ6*P(18,23) + HKZ7*P(16,23) - HKZ8*P(17,23) + HKZ9*P(3,23) + P(21,23));


+// Predicted observation
+
+
+// Innovation variance
+
+
@@ -75,6 +75,12 @@ Kfusion(22) = HK33*(-HK12*P(2,22) - HK13*P(6,22) + HK29 - HK7*P(22,22));
 Kfusion(23) = HK33*(-HK12*P(2,23) - HK13*P(6,23) + HK24 - HK7*P(22,23));


+// Predicted observation
+
+
+// Innovation variance
+
+
 // Axis 1 equations
 // Sub Expressions
 const float HK0 = ve - vwe;
@@ -152,3 +158,9 @@ Kfusion(22) = HK33*(-HK12*P(3,22) + HK24 - HK25 - HK9*P(22,23));
 Kfusion(23) = HK33*(-HK12*P(3,23) - HK13*P(4,23) + HK29 - HK9*P(23,23));


+// Predicted observation
+
+
+// Innovation variance
+
+
@@ -91,3 +91,9 @@ Kfusion(22) = HK43*HK50;
 Kfusion(23) = HK40*HK50;


+// Predicted observation
+
+
+// Innovation variance
+
+
@@ -81,6 +81,12 @@ Kfusion(22) = HK41*(HK22*P(4,22) + HK27*P(5,22) + HK28*P(6,22) + HK29*P(0,22) +
 Kfusion(23) = HK41*(HK22*P(4,23) + HK27*P(5,23) + HK28*P(6,23) + HK29*P(0,23) + HK30*P(1,23) + HK31*P(2,23) + HK32*P(3,23));


+// Predicted observation
+
+
+// Innovation variance
+
+
 // Y Axis Equations
 // Sub Expressions
 const float HK0 = -Tbs(0,0)*q2 + Tbs(0,1)*q1 + Tbs(0,2)*q0;
@@ -170,3 +176,9 @@ Kfusion(22) = -HK47*(HK32*P(0,22) + HK33*P(1,22) + HK34*P(2,22) + HK35*P(3,22) +
 Kfusion(23) = -HK47*(HK32*P(0,23) + HK33*P(1,23) + HK34*P(2,23) + HK35*P(3,23) + HK36*P(4,23) + HK37*P(5,23) + HK38*P(6,23));


+// Predicted observation
+
+
+// Innovation variance
+
+
@@ -65,3 +65,9 @@ Kfusion(22) = HK29*(-HK14*P(0,22) - HK20*P(1,22) - HK21*P(2,22) + HK22*HK8*P(3,2
 Kfusion(23) = HK29*(-HK14*P(0,23) - HK20*P(1,23) - HK21*P(2,23) + HK22*HK8*P(3,23));


+// Predicted observation
+
+
+// Innovation variance
+
+
@@ -43,3 +43,9 @@ Kfusion(22) = -HK9*(HK5*P(16,22) - P(17,22));
 Kfusion(23) = -HK9*(HK5*P(16,23) - P(17,23));


+// Predicted observation
+
+
+// Innovation variance
+
+
@@ -53,3 +53,9 @@ Kfusion(22) = HK15*HK16;
 Kfusion(23) = HK14*HK16;


+// Predicted observation
+
+
+// Innovation variance
+
+
@@ -0,0 +1,55 @@
+// Sub Expressions
+const float HK0 = vn - vwn;
+const float HK1 = ve - vwe;
+const float HK2 = sqrtf((HK0)*(HK0) + (HK1)*(HK1) + (vd)*(vd));
+const float HK3 = 1.0F/(HK2);
+const float HK4 = HK0*HK3;
+const float HK5 = HK1*HK3;
+const float HK6 = HK3*vd;
+const float HK7 = -HK0*HK3;
+const float HK8 = -HK1*HK3;
+const float HK9 = 1.0F/(innov_var);
+
+
+// Observation Jacobians
+Hfusion.at<4>() = HK4;
+Hfusion.at<5>() = HK5;
+Hfusion.at<6>() = HK6;
+Hfusion.at<22>() = HK7;
+Hfusion.at<23>() = HK8;
+
+
+// Kalman gains
+Kfusion(0) = HK9*(HK4*P(0,4) + HK5*P(0,5) + HK6*P(0,6) + HK7*P(0,22) + HK8*P(0,23));
+Kfusion(1) = HK9*(HK4*P(1,4) + HK5*P(1,5) + HK6*P(1,6) + HK7*P(1,22) + HK8*P(1,23));
+Kfusion(2) = HK9*(HK4*P(2,4) + HK5*P(2,5) + HK6*P(2,6) + HK7*P(2,22) + HK8*P(2,23));
+Kfusion(3) = HK9*(HK4*P(3,4) + HK5*P(3,5) + HK6*P(3,6) + HK7*P(3,22) + HK8*P(3,23));
+Kfusion(4) = HK9*(HK4*P(4,4) + HK5*P(4,5) + HK6*P(4,6) + HK7*P(4,22) + HK8*P(4,23));
+Kfusion(5) = HK9*(HK4*P(4,5) + HK5*P(5,5) + HK6*P(5,6) + HK7*P(5,22) + HK8*P(5,23));
+Kfusion(6) = HK9*(HK4*P(4,6) + HK5*P(5,6) + HK6*P(6,6) + HK7*P(6,22) + HK8*P(6,23));
+Kfusion(7) = HK9*(HK4*P(4,7) + HK5*P(5,7) + HK6*P(6,7) + HK7*P(7,22) + HK8*P(7,23));
+Kfusion(8) = HK9*(HK4*P(4,8) + HK5*P(5,8) + HK6*P(6,8) + HK7*P(8,22) + HK8*P(8,23));
+Kfusion(9) = HK9*(HK4*P(4,9) + HK5*P(5,9) + HK6*P(6,9) + HK7*P(9,22) + HK8*P(9,23));
+Kfusion(10) = HK9*(HK4*P(4,10) + HK5*P(5,10) + HK6*P(6,10) + HK7*P(10,22) + HK8*P(10,23));
+Kfusion(11) = HK9*(HK4*P(4,11) + HK5*P(5,11) + HK6*P(6,11) + HK7*P(11,22) + HK8*P(11,23));
+Kfusion(12) = HK9*(HK4*P(4,12) + HK5*P(5,12) + HK6*P(6,12) + HK7*P(12,22) + HK8*P(12,23));
+Kfusion(13) = HK9*(HK4*P(4,13) + HK5*P(5,13) + HK6*P(6,13) + HK7*P(13,22) + HK8*P(13,23));
+Kfusion(14) = HK9*(HK4*P(4,14) + HK5*P(5,14) + HK6*P(6,14) + HK7*P(14,22) + HK8*P(14,23));
+Kfusion(15) = HK9*(HK4*P(4,15) + HK5*P(5,15) + HK6*P(6,15) + HK7*P(15,22) + HK8*P(15,23));
+Kfusion(16) = HK9*(HK4*P(4,16) + HK5*P(5,16) + HK6*P(6,16) + HK7*P(16,22) + HK8*P(16,23));
+Kfusion(17) = HK9*(HK4*P(4,17) + HK5*P(5,17) + HK6*P(6,17) + HK7*P(17,22) + HK8*P(17,23));
+Kfusion(18) = HK9*(HK4*P(4,18) + HK5*P(5,18) + HK6*P(6,18) + HK7*P(18,22) + HK8*P(18,23));
+Kfusion(19) = HK9*(HK4*P(4,19) + HK5*P(5,19) + HK6*P(6,19) + HK7*P(19,22) + HK8*P(19,23));
+Kfusion(20) = HK9*(HK4*P(4,20) + HK5*P(5,20) + HK6*P(6,20) + HK7*P(20,22) + HK8*P(20,23));
+Kfusion(21) = HK9*(HK4*P(4,21) + HK5*P(5,21) + HK6*P(6,21) + HK7*P(21,22) + HK8*P(21,23));
+Kfusion(22) = HK9*(HK4*P(4,22) + HK5*P(5,22) + HK6*P(6,22) + HK7*P(22,22) + HK8*P(22,23));
+Kfusion(23) = HK9*(HK4*P(4,23) + HK5*P(5,23) + HK6*P(6,23) + HK7*P(22,23) + HK8*P(23,23));
+
+
+// Predicted observation
+meas_pred = HK2;
+
+
+// Innovation variance
+
+
@@ -0,0 +1,25 @@
+// Sub Expressions
+const float IV0 = ve - vwe;
+const float IV1 = vn - vwn;
+const float IV2 = (IV0)*(IV0) + (IV1)*(IV1) + (vd)*(vd);
+const float IV3 = 1.0F/(IV2);
+const float IV4 = IV0*P(5,23);
+const float IV5 = IV0*IV3;
+const float IV6 = IV1*P(4,22);
+const float IV7 = IV1*IV3;
+
+
+// Observation Jacobians
+
+
+// Kalman gains
+
+
+// Predicted observation
+meas_pred = sqrtf(IV2);
+
+
+// Innovation variance
+innov_var = IV3*vd*(IV0*P(5,6) - IV0*P(6,23) + IV1*P(4,6) - IV1*P(6,22) + P(6,6)*vd) - IV5*(-IV0*P(23,23) - IV1*P(22,23) + IV1*P(4,23) + IV4 + P(6,23)*vd) + IV5*(IV0*P(5,5) + IV1*P(4,5) - IV1*P(5,22) - IV4 + P(5,6)*vd) - IV7*(-IV0*P(22,23) + IV0*P(5,22) - IV1*P(22,22) + IV6 + P(6,22)*vd) + IV7*(-IV0*P(4,23) + IV0*P(4,5) + IV1*P(4,4) - IV6 + P(4,6)*vd) + R_TAS;
+
+
@@ -101,6 +101,27 @@ def generate_observation_equations(P,state,observation,variance,varname="HK"):

    return HK_simple

+def generate_observation_equations_innov_var_only(P,state,observation,variance,varname="IV"):
+    H = Matrix([observation]).jacobian(state)
+    innov_var = H * P * H.T + Matrix([variance])
+    assert(innov_var.shape[0] == 1)
+    assert(innov_var.shape[1] == 1)
+    extension="0:1000"
+    var_string = varname+extension
+    IV_simple = cse(Matrix([zeros(24,1), zeros(24,1), observation, innov_var]), symbols(var_string), optimizations='basic')
+
+    return IV_simple
+
+def generate_observation_equations_hk_only(P,state,observation,varname="HK"):
+    H = Matrix([observation]).jacobian(state)
+    K = P * H.T / Symbol("innov_var")
+    extension="0:1000"
+    var_string = varname+extension
+    # optimizations=None produces a set of equations that fits nicely in a for-loop
+    HK_simple = cse(Matrix([H.transpose(), K, observation]), symbols(var_string), optimizations=None)
+
+    return HK_simple
+
 # generate equations for observation vector Jacobian and Kalman gain
 # n_obs is the vector dimension and must be >= 2
 def generate_observation_vector_equations(P,state,observation,variance,n_obs):
@@ -130,7 +151,11 @@ def write_equations_to_file(equations,code_generator_id,n_obs):
        code_generator_id.print_string("Observation Jacobians")
        code_generator_id.write_matrix(Matrix(equations[1][0][0:24]), "Hfusion", False, ".at<", ">()")
        code_generator_id.print_string("Kalman gains")
-        code_generator_id.write_matrix(Matrix(equations[1][0][24:]), "Kfusion", False, "(", ")")
+        code_generator_id.write_matrix(Matrix(equations[1][0][24:48]), "Kfusion", False, "(", ")")
+        code_generator_id.print_string("Predicted observation")
+        code_generator_id.write_matrix(Matrix(equations[1][0][48:49]), "meas_pred")
+        code_generator_id.print_string("Innovation variance")
+        code_generator_id.write_matrix(Matrix(equations[1][0][49:50]), "innov_var", False, "(", ")")
    else:
        code_generator_id.print_string("Sub Expressions")
        code_generator_id.write_subexpressions(equations[0])
@@ -404,6 +429,18 @@ def tas_observation(P,state,vx,vy,vz,wx,wy):
    write_equations_to_file(equations,tas_code_generator,1)
    tas_code_generator.close()

+    equations = generate_observation_equations_innov_var_only(P,state,observation,obs_var)
+
+    tas_code_generator = CodeGenerator("./generated/tas_var_generated.cpp")
+    write_equations_to_file(equations,tas_code_generator,1)
+    tas_code_generator.close()
+
+    equations = generate_observation_equations_hk_only(P,state,observation)
+
+    tas_code_generator = CodeGenerator("./generated/tas_hk_generated.cpp")
+    write_equations_to_file(equations,tas_code_generator,1)
+    tas_code_generator.close()
+
    return

 # sideslip fusion