diff --git a/EKF/EKFGSF_yaw.cpp b/EKF/EKFGSF_yaw.cpp index e4681b30ad..ec6e179c41 100644 --- a/EKF/EKFGSF_yaw.cpp +++ b/EKF/EKFGSF_yaw.cpp @@ -277,54 +277,50 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index) _ekf_gsf[model_index].X(0) += del_vel_NED(0); _ekf_gsf[model_index].X(1) += del_vel_NED(1); - // predict covariance - autocode from https://github.com/priseborough/3_state_filter/blob/flightLogReplay-wip/calcPupdate.txt + // predict covariance - equations generated using EKF/python/gsf_ekf_yaw_estimator/main.py // Local short variable name copies required for readability - // Compiler might be smart enough to optimise these out const float &P00 = _ekf_gsf[model_index].P(0,0); const float &P01 = _ekf_gsf[model_index].P(0,1); const float &P02 = _ekf_gsf[model_index].P(0,2); - const float &P10 = _ekf_gsf[model_index].P(1,0); const float &P11 = _ekf_gsf[model_index].P(1,1); const float &P12 = _ekf_gsf[model_index].P(1,2); - const float &P20 = _ekf_gsf[model_index].P(2,0); - const float &P21 = _ekf_gsf[model_index].P(2,1); const float &P22 = _ekf_gsf[model_index].P(2,2); + const float &psi = _ekf_gsf[model_index].X(2); // Use fixed values for delta velocity and delta angle process noise variances const float dvxVar = sq(_accel_noise * _delta_vel_dt); // variance of forward delta velocity - (m/s)^2 const float dvyVar = dvxVar; // variance of right delta velocity - (m/s)^2 const float dazVar = sq(_gyro_noise * _delta_ang_dt); // variance of yaw delta angle - rad^2 - const float t2 = sinf(_ekf_gsf[model_index].X(2)); - const float t3 = cosf(_ekf_gsf[model_index].X(2)); - const float t4 = dvy*t3; - const float t5 = dvx*t2; - const float t6 = t4+t5; - const float t8 = P22*t6; - const float t7 = P02-t8; - const float t9 = dvx*t3; - const float t11 = dvy*t2; - const float t10 = t9-t11; - const float t12 = dvxVar*t2*t3; - const float t13 = t2*t2; - const float t14 = t3*t3; - const float t15 = P22*t10; - const float t16 = P12+t15; + const float S0 = cosf(psi); + const float S1 = ecl::powf(S0, 2); + const float S2 = sinf(psi); + const float S3 = ecl::powf(S2, 2); + const float S4 = S0*dvy + S2*dvx; + const float S5 = P02 - P22*S4; + const float S6 = S0*dvx - S2*dvy; + const float S7 = S0*S2; + const float S8 = P01 + S7*dvxVar - S7*dvyVar; + const float S9 = P12 + P22*S6; - constexpr float min_var = 1e-6f; - _ekf_gsf[model_index].P(0,0) = fmaxf(P00-P20*t6+dvxVar*t14+dvyVar*t13-t6*t7 , min_var); - _ekf_gsf[model_index].P(0,1) = P01+t12-P21*t6+t7*t10-dvyVar*t2*t3; - _ekf_gsf[model_index].P(0,2) = t7; - _ekf_gsf[model_index].P(1,0) = P10+t12+P20*t10-t6*t16-dvyVar*t2*t3; - _ekf_gsf[model_index].P(1,1) = fmaxf(P11+P21*t10+dvxVar*t13+dvyVar*t14+t10*t16 , min_var); - _ekf_gsf[model_index].P(1,2) = t16; - _ekf_gsf[model_index].P(2,0) = P20-t8; - _ekf_gsf[model_index].P(2,1) = P21+t15; - _ekf_gsf[model_index].P(2,2) = fmaxf(P22+dazVar , min_var); + _ekf_gsf[model_index].P(0,0) = P00 - P02*S4 + S1*dvxVar + S3*dvyVar - S4*S5; + _ekf_gsf[model_index].P(0,1) = -P12*S4 + S5*S6 + S8; + _ekf_gsf[model_index].P(1,1) = P11 + P12*S6 + S1*dvyVar + S3*dvxVar + S6*S9; + _ekf_gsf[model_index].P(0,2) = S5; + _ekf_gsf[model_index].P(1,2) = S9; + _ekf_gsf[model_index].P(2,2) = P22 + dazVar; - // force symmetry - _ekf_gsf[model_index].P.makeBlockSymmetric<3>(0); + // covariance matrix is symmetrical, so copy upper half to lower half + _ekf_gsf[model_index].P(1,0) = _ekf_gsf[model_index].P(0,1); + _ekf_gsf[model_index].P(2,0) = _ekf_gsf[model_index].P(0,2); + _ekf_gsf[model_index].P(2,1) = _ekf_gsf[model_index].P(1,2); + + // constrain variances + const float min_var = 1e-6f; + for (unsigned index = 0; index < 3; index++) { + _ekf_gsf[model_index].P(index,index) = fmaxf(_ekf_gsf[model_index].P(index,index),min_var); + } } // Update EKF states and covariance for specified model index using velocity measurement @@ -341,20 +337,80 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index) const float &P00 = _ekf_gsf[model_index].P(0,0); const float &P01 = _ekf_gsf[model_index].P(0,1); const float &P02 = _ekf_gsf[model_index].P(0,2); - const float &P10 = _ekf_gsf[model_index].P(1,0); const float &P11 = _ekf_gsf[model_index].P(1,1); const float &P12 = _ekf_gsf[model_index].P(1,2); - const float &P20 = _ekf_gsf[model_index].P(2,0); - const float &P21 = _ekf_gsf[model_index].P(2,1); const float &P22 = _ekf_gsf[model_index].P(2,2); - // calculate innovation variance - matrix::SquareMatrix S = _ekf_gsf[model_index].P.slice<2, 2>(0, 0); - S(0, 0) += velObsVar; - S(1, 1) += velObsVar; + // optimized auto generated code + const float t0 = ecl::powf(P01, 2); + const float t1 = -t0; + const float t2 = P00*P11 + P00*velObsVar + P11*velObsVar + t1 + ecl::powf(velObsVar, 2); + if (fabsf(t2) < 1e-6f) { + return false; + } + const float t3 = 1.0F/t2; + const float t4 = P11 + velObsVar; + const float t5 = P01*t3; + const float t6 = -t5; + const float t7 = P00 + velObsVar; + const float t8 = P00*t4 + t1; + const float t9 = t5*velObsVar; + const float t10 = P11*t7; + const float t11 = t1 + t10; + const float t12 = P01*P12; + const float t13 = P02*t4; + const float t14 = P01*P02; + const float t15 = P12*t7; + const float t16 = t0*velObsVar; + const float t17 = ecl::powf(t2, -2); + const float t18 = t4*velObsVar + t8; + const float t19 = t17*t18; + const float t20 = t17*(t16 + t7*t8); + const float t21 = t0 - t10; + const float t22 = t17*t21; + const float t23 = t14 - t15; + const float t24 = P01*t23; + const float t25 = t12 - t13; + const float t26 = t16 - t21*t4; + const float t27 = t17*t26; + const float t28 = t11 + t7*velObsVar; + const float t30 = t17*t28; + const float t31 = P01*t25; + const float t32 = t23*t4 + t31; + const float t33 = t17*t32; + const float t35 = t24 + t25*t7; + const float t36 = t17*t35; - // Update the inverse of the innovation covariance matrix S_inverse - updateInnovCovMatInv(model_index, S); + _ekf_gsf[model_index].S_det_inverse = t3; + + _ekf_gsf[model_index].S_inverse(0,0) = t3*t4; + _ekf_gsf[model_index].S_inverse(0,1) = t6; + _ekf_gsf[model_index].S_inverse(1,1) = t3*t7; + _ekf_gsf[model_index].S_inverse(1,0) = _ekf_gsf[model_index].S_inverse(0,1); + + matrix::Matrix K; + K(0,0) = t3*t8; + K(1,0) = t9; + K(2,0) = t3*(-t12 + t13); + K(0,1) = t9; + K(1,1) = t11*t3; + K(2,1) = t3*(-t14 + t15); + + _ekf_gsf[model_index].P(0,0) = P00 - t16*t19 - t20*t8; + _ekf_gsf[model_index].P(0,1) = P01*(t18*t22 - t20*velObsVar + 1); + _ekf_gsf[model_index].P(1,1) = P11 - t16*t30 + t22*t26; + _ekf_gsf[model_index].P(0,2) = P02 + t19*t24 + t20*t25; + _ekf_gsf[model_index].P(1,2) = P12 + t23*t27 + t30*t31; + _ekf_gsf[model_index].P(2,2) = P22 - t23*t33 - t25*t36; + _ekf_gsf[model_index].P(1,0) = _ekf_gsf[model_index].P(0,1); + _ekf_gsf[model_index].P(2,0) = _ekf_gsf[model_index].P(0,2); + _ekf_gsf[model_index].P(2,1) = _ekf_gsf[model_index].P(1,2); + + // constrain variances + const float min_var = 1e-6f; + for (unsigned index = 0; index < 3; index++) { + _ekf_gsf[model_index].P(index,index) = fmaxf(_ekf_gsf[model_index].P(index,index),min_var); + } // test ratio = transpose(innovation) * inverse(innovation variance) * innovation = [1x2] * [2,2] * [2,1] = [1,1] const float test_ratio = _ekf_gsf[model_index].innov * (_ekf_gsf[model_index].S_inverse * _ekf_gsf[model_index].innov); @@ -365,81 +421,6 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index) // This protects from large measurement spikes const float innov_comp_scale_factor = test_ratio > 25.f ? sqrtf(25.0f / test_ratio) : 1.f; - // calculate Kalman gain K nd covariance matrix P - // autocode from https://github.com/priseborough/3_state_filter/blob/flightLogReplay-wip/calcK.txt - // and https://github.com/priseborough/3_state_filter/blob/flightLogReplay-wip/calcPmat.txt - const float t2 = P00*velObsVar; - const float t3 = P11*velObsVar; - const float t4 = velObsVar*velObsVar; - const float t5 = P00*P11; - const float t9 = P01*P10; - const float t6 = t2+t3+t4+t5-t9; - if (fabsf(t6) < 1e-6f) { - // skip the fusion step - return false; - } - const float t7 = 1.f / t6; - const float t8 = P11+velObsVar; - const float t10 = P00+velObsVar; - - matrix::Matrix K; - K(0,0) = -P01*P10*t7+P00*t7*t8; - K(0,1) = -P00*P01*t7+P01*t7*t10; - K(1,0) = -P10*P11*t7+P10*t7*t8; - K(1,1) = -P01*P10*t7+P11*t7*t10; - K(2,0) = -P10*P21*t7+P20*t7*t8; - K(2,1) = -P01*P20*t7+P21*t7*t10; - - const float t11 = P00*P01*t7; - const float t15 = P01*t7*t10; - const float t12 = t11-t15; - const float t13 = P01*P10*t7; - const float t16 = P00*t7*t8; - const float t14 = t13-t16; - const float t17 = t8*t12; - const float t18 = P01*t14; - const float t19 = t17+t18; - const float t20 = t10*t14; - const float t21 = P10*t12; - const float t22 = t20+t21; - const float t27 = P11*t7*t10; - const float t23 = t13-t27; - const float t24 = P10*P11*t7; - const float t26 = P10*t7*t8; - const float t25 = t24-t26; - const float t28 = t8*t23; - const float t29 = P01*t25; - const float t30 = t28+t29; - const float t31 = t10*t25; - const float t32 = P10*t23; - const float t33 = t31+t32; - const float t34 = P01*P20*t7; - const float t38 = P21*t7*t10; - const float t35 = t34-t38; - const float t36 = P10*P21*t7; - const float t39 = P20*t7*t8; - const float t37 = t36-t39; - const float t40 = t8*t35; - const float t41 = P01*t37; - const float t42 = t40+t41; - const float t43 = t10*t37; - const float t44 = P10*t35; - const float t45 = t43+t44; - - const float min_var = 1e-6f; - _ekf_gsf[model_index].P(0,0) = fmaxf(P00-t12*t19-t14*t22 , min_var); - _ekf_gsf[model_index].P(0,1) = P01-t19*t23-t22*t25; - _ekf_gsf[model_index].P(0,2) = P02-t19*t35-t22*t37; - _ekf_gsf[model_index].P(1,0) = P10-t12*t30-t14*t33; - _ekf_gsf[model_index].P(1,1) = fmaxf(P11-t23*t30-t25*t33 , min_var); - _ekf_gsf[model_index].P(1,2) = P12-t30*t35-t33*t37; - _ekf_gsf[model_index].P(2,0) = P20-t12*t42-t14*t45; - _ekf_gsf[model_index].P(2,1) = P21-t23*t42-t25*t45; - _ekf_gsf[model_index].P(2,2) = fmaxf(P22-t35*t42-t37*t45 , min_var); - - // force symmetry - _ekf_gsf[model_index].P.makeBlockSymmetric<3>(0); - // Correct the state vector and capture the change in yaw angle const float oldYaw = _ekf_gsf[model_index].X(2); @@ -497,24 +478,6 @@ float EKFGSF_yaw::gaussianDensity(const uint8_t model_index) const return _m_2pi_inv * sqrtf(_ekf_gsf[model_index].S_det_inverse) * expf(-0.5f * normDist); } -void EKFGSF_yaw::updateInnovCovMatInv(const uint8_t model_index, const matrix::SquareMatrix &S) -{ - // calculate determinant for innovation covariance matrix - const float t2 = S(0,0) * S(1,1); - const float t5 = S(0,1) * S(1,0); - const float t3 = t2 - t5; - - // calculate determinant inverse and protect against badly conditioned matrix - _ekf_gsf[model_index].S_det_inverse = 1.0f / fmaxf(t3 , 1e-12f); - - // calculate inv(S) - _ekf_gsf[model_index].S_inverse(0,0) = _ekf_gsf[model_index].S_det_inverse * S(1,1); - _ekf_gsf[model_index].S_inverse(1,1) = _ekf_gsf[model_index].S_det_inverse * S(0,0); - _ekf_gsf[model_index].S_inverse(0,1) = - _ekf_gsf[model_index].S_det_inverse * S(0,1); - _ekf_gsf[model_index].S_inverse(1,0) = - _ekf_gsf[model_index].S_det_inverse * S(1,0); - -} - bool EKFGSF_yaw::getLogData(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) { if (_ekf_gsf_vel_fuse_started) { diff --git a/EKF/EKFGSF_yaw.h b/EKF/EKFGSF_yaw.h index 5f6a1d9687..9b07ae6f6d 100644 --- a/EKF/EKFGSF_yaw.h +++ b/EKF/EKFGSF_yaw.h @@ -133,8 +133,4 @@ private: // return the probability of the state estimate for the specified EKF assuming a gaussian error distribution float gaussianDensity(const uint8_t model_index) const; - - // update the inverse of the innovation covariance matrix - void updateInnovCovMatInv(const uint8_t model_index, const matrix::SquareMatrix &S); - }; diff --git a/EKF/python/gsf_ekf_yaw_estimator/code_gen.py b/EKF/python/gsf_ekf_yaw_estimator/code_gen.py new file mode 100644 index 0000000000..21d9794093 --- /dev/null +++ b/EKF/python/gsf_ekf_yaw_estimator/code_gen.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Sat Mar 14 12:47:24 2020 + +@author: roman +""" +from sympy import ccode +from sympy.codegen.ast import float32, real + +class CodeGenerator: + def __init__(self, file_name): + self.file_name = file_name + self.file = open(self.file_name, 'w') + + def print_string(self, string): + self.file.write("// " + string + "\n") + + def get_ccode(self, expression): + return ccode(expression, type_aliases={real:float32}) + + def write_subexpressions(self,subexpressions): + output_string = "" + for item in subexpressions: + output_string = output_string + "const float " + str(item[0]) + " = " + self.get_ccode(item[1]) + ";\n" + + output_string = output_string + "\n\n" + self.file.write(output_string) + + def write_matrix(self, matrix, identifier, is_symmetric=False): + output_string = "" + + if matrix.shape[0] * matrix.shape[1] == 1: + output_string = output_string + identifier + " = " + self.get_ccode(matrix[0]) + ";\n" + elif matrix.shape[0] == 1 or matrix.shape[1] == 1: + for i in range(0,len(matrix)): + if (identifier == "Kfusion"): + # Vector f format used by Kfusion + output_string = output_string + identifier + "(" + str(i) + ") = " + self.get_ccode(matrix[i]) + ";\n" + else: + # legacy array format used by Hfusion + output_string = output_string + identifier + "[" + str(i) + "] = " + self.get_ccode(matrix[i]) + ";\n" + else: + for j in range(0, matrix.shape[1]): + for i in range(0, matrix.shape[0]): + if j >= i or not is_symmetric: + output_string = output_string + identifier + "(" + str(i) + "," + str(j) + ") = " + self.get_ccode(matrix[i,j]) + ";\n" + # legacy array format + # output_string = output_string + identifier + "[" + str(i) + "][" + str(j) + "] = " + self.get_ccode(matrix[i,j]) + ";\n" + if is_symmetric: + output_string = output_string + "\n" + for j in range(0, matrix.shape[1]): + for i in range(0, matrix.shape[0]): + if j < i: + output_string = output_string + identifier + "(" + str(i) + "," + str(j) + ") = " + \ + identifier + "(" + str(j) + "," + str(i) + ")" + ";\n" + + output_string = output_string + "\n\n" + self.file.write(output_string) + + def close(self): + self.file.close() diff --git a/EKF/python/gsf_ekf_yaw_estimator/generated/covariance_prediction_generated.cpp b/EKF/python/gsf_ekf_yaw_estimator/generated/covariance_prediction_generated.cpp new file mode 100644 index 0000000000..a8071f236d --- /dev/null +++ b/EKF/python/gsf_ekf_yaw_estimator/generated/covariance_prediction_generated.cpp @@ -0,0 +1,25 @@ +// Equations for covariance matrix prediction +const float S0 = cosf(psi); +const float S1 = powf(S0, 2); +const float S2 = sinf(psi); +const float S3 = powf(S2, 2); +const float S4 = S0*dvy + S2*dvx; +const float S5 = P02 - P22*S4; +const float S6 = S0*dvx - S2*dvy; +const float S7 = S0*S2; +const float S8 = P01 + S7*dvxVar - S7*dvyVar; +const float S9 = P12 + P22*S6; + + +_ekf_gsf[model_index].P(0,0) = P00 - P02*S4 + S1*dvxVar + S3*dvyVar - S4*S5; +_ekf_gsf[model_index].P(0,1) = -P12*S4 + S5*S6 + S8; +_ekf_gsf[model_index].P(1,1) = P11 + P12*S6 + S1*dvyVar + S3*dvxVar + S6*S9; +_ekf_gsf[model_index].P(0,2) = S5; +_ekf_gsf[model_index].P(1,2) = S9; +_ekf_gsf[model_index].P(2,2) = P22 + dazVar; + +_ekf_gsf[model_index].P(1,0) = _ekf_gsf[model_index].P(0,1); +_ekf_gsf[model_index].P(2,0) = _ekf_gsf[model_index].P(0,2); +_ekf_gsf[model_index].P(2,1) = _ekf_gsf[model_index].P(1,2); + + diff --git a/EKF/python/gsf_ekf_yaw_estimator/generated/measurement_update_generated.cpp b/EKF/python/gsf_ekf_yaw_estimator/generated/measurement_update_generated.cpp new file mode 100644 index 0000000000..7e2ff07e69 --- /dev/null +++ b/EKF/python/gsf_ekf_yaw_estimator/generated/measurement_update_generated.cpp @@ -0,0 +1,74 @@ +// Intermediate variables +const float t0 = powf(P01, 2); +const float t1 = -t0; +const float t2 = P00*P11 + P00*velObsVar + P11*velObsVar + t1 + powf(velObsVar, 2); +const float t3 = 1.0F/t2; +const float t4 = P11 + velObsVar; +const float t5 = P01*t3; +const float t6 = -t5; +const float t7 = P00 + velObsVar; +const float t8 = P00*t4 + t1; +const float t9 = t5*velObsVar; +const float t10 = P11*t7; +const float t11 = t1 + t10; +const float t12 = P01*P12; +const float t13 = P02*t4; +const float t14 = P01*P02; +const float t15 = P12*t7; +const float t16 = t0*velObsVar; +const float t17 = powf(t2, -2); +const float t18 = t4*velObsVar + t8; +const float t19 = t17*t18; +const float t20 = t17*(t16 + t7*t8); +const float t21 = t0 - t10; +const float t22 = t17*t21; +const float t23 = t14 - t15; +const float t24 = P01*t23; +const float t25 = t12 - t13; +const float t26 = t16 - t21*t4; +const float t27 = t17*t26; +const float t28 = t11 + t7*velObsVar; +const float t29 = t17*t8; // unused variable +const float t30 = t17*t28; +const float t31 = P01*t25; +const float t32 = t23*t4 + t31; +const float t33 = t17*t32; +const float t34 = P01*velObsVar; // Unused variable +const float t35 = t24 + t25*t7; +const float t36 = t17*t35; + + +// Equations for NE velocity innovation variance's determinante inverse +_ekf_gsf[model_index].S_det_inverse = t3; + + +// Equations for NE velocity innovation variance inverse +_ekf_gsf[model_index].S_inverse(0,0) = t3*t4; +_ekf_gsf[model_index].S_inverse(0,1) = t6; +_ekf_gsf[model_index].S_inverse(1,1) = t3*t7; + +_ekf_gsf[model_index].S_inverse(1,0) = _ekf_gsf[model_index].S_inverse(0,1); + + +// Equations for NE velocity Kalman gain +K(0,0) = t3*t8; +K(1,0) = t9; +K(2,0) = t3*(-t12 + t13); +K(0,1) = t9; +K(1,1) = t11*t3; +K(2,1) = t3*(-t14 + t15); + + +// Equations for covariance matrix update +_ekf_gsf[model_index].P(0,0) = P00 - t16*t19 - t20*t8; +_ekf_gsf[model_index].P(0,1) = P01*(t18*t22 - t20*velObsVar + 1); +_ekf_gsf[model_index].P(1,1) = P11 - t16*t30 + t22*t26; +_ekf_gsf[model_index].P(0,2) = P02 + t19*t24 + t20*t25; +_ekf_gsf[model_index].P(1,2) = P12 + t23*t27 + t30*t31; +_ekf_gsf[model_index].P(2,2) = P22 - t23*t33 - t25*t36; + +_ekf_gsf[model_index].P(1,0) = _ekf_gsf[model_index].P(0,1); +_ekf_gsf[model_index].P(2,0) = _ekf_gsf[model_index].P(0,2); +_ekf_gsf[model_index].P(2,1) = _ekf_gsf[model_index].P(1,2); + + diff --git a/EKF/python/gsf_ekf_yaw_estimator/main.py b/EKF/python/gsf_ekf_yaw_estimator/main.py new file mode 100644 index 0000000000..3ec12af399 --- /dev/null +++ b/EKF/python/gsf_ekf_yaw_estimator/main.py @@ -0,0 +1,112 @@ +from sympy import * +from code_gen import * + +def create_cov_matrix(i, j): + if j >= i: + return Symbol("P" + str(i) + str(j), real=True) + else: + return 0 + +def create_symmetric_cov_matrix(): + # define a symbolic covariance matrix + P = Matrix(3,3,create_cov_matrix) + + for index in range(3): + for j in range(3): + if index > j: + P[index,j] = P[j,index] + + return P + +print('Starting code generation:') + +dt = symbols("dt", real=True) # dt (sec) +psi = symbols("psi", real=True) # yaw angle of body frame wrt earth frame +vn, ve = symbols("vn ve", real=True) # velocity in world frame (north/east) - m/sec +daz = symbols("daz", real=True) # IMU z axis delta angle measurement in body axes - rad +dazVar = symbols("dazVar", real=True) # IMU Z axis delta angle measurement variance (rad^2) +dvx, dvy = symbols("dvx dvy", real=True) # IMU x and y axis delta velocity measurement in body axes - m/sec +dvxVar, dvyVar = symbols("dvxVar dvyVar", real=True) # IMU x and y axis delta velocity measurement variance (m/s)^2 + +# derive the body to nav direction transformation matrix +Tbn = Matrix([[cos(psi) , -sin(psi)], + [sin(psi) , cos(psi)]]) + +# attitude update equation +psiNew = psi + daz + +# velocity update equations +velNew = Matrix([vn,ve]) + Tbn*Matrix([dvx,dvy]) + +# Define the state vectors +stateVector = Matrix([vn,ve,psi]) + +# Define vector of process equations +newStateVector = Matrix([velNew,psiNew]) + +# Calculate state transition matrix +print('Computing state propagation jacobian ...') +F = newStateVector.jacobian(stateVector) + +# Derive the covariance prediction equations +# Error growth in the inertial solution is assumed to be driven by 'noise' in the delta angles and +# velocities, after bias effects have been removed. + +# derive the control(disturbance) influence matrix from IMU noise to state noise +G = newStateVector.jacobian(Matrix([dvx,dvy,daz])) + +# derive the state error matrix +distMatrix = Matrix([[dvxVar , 0 , 0], + [0 , dvyVar , 0], + [0 , 0 , dazVar]]) + +Q = G * distMatrix * G.T + +# propagate covariance matrix +P = create_symmetric_cov_matrix() + +print('Computing covariance propagation ...') +P_new = F * P * F.T + Q + +print('Simplifying covariance propagation ...') +P_new_simple = cse(P_new, symbols("S0:1000"), optimizations='basic') + +print('Writing covariance propagation to file ...') +cov_prediction_code_generator = CodeGenerator("./generated/covariance_prediction_generated.cpp") +cov_prediction_code_generator.print_string("Equations for covariance matrix prediction") +cov_prediction_code_generator.write_subexpressions(P_new_simple[0]) +cov_prediction_code_generator.write_matrix(Matrix(P_new_simple[1]), "_ekf_gsf[model_index].P", True) +cov_prediction_code_generator.close() + +# derive the covariance update equation for a NE velocity observation +print('Computing NE velocity observatio innovation variance code ...') +velObsVar = symbols("velObsVar", real=True) # velocity observation variance (m/s)^2 +H = Matrix([[1,0,0], + [0,1,0]]) + +R = Matrix([[velObsVar , 0], + [0 , velObsVar]]) + +print('Computing NE velocity measurement update code ...') +S = H * P * H.T + R +S_det_inv = 1 / S.det() +S_inv = S.inv() +K = (P * H.T) * S_inv +P_new = P - K * S * K.T + +# optimize code +t, [S_det_inv_s, S_inv_s, K_s, P_new_s] = cse([S_det_inv, S_inv, K, P_new], symbols("t0:1000"), optimizations='basic') + +print('Writing NE velocity measurement update code to file ...') +measurement_update_code_generator = CodeGenerator("./generated/measurement_update_generated.cpp") +measurement_update_code_generator.print_string("Intermediate variables") +measurement_update_code_generator.write_subexpressions(t) +measurement_update_code_generator.print_string("Equations for NE velocity innovation variance's determinante inverse") +measurement_update_code_generator.write_matrix(Matrix([[S_det_inv_s]]), "_ekf_gsf[model_index].S_det_inverse", False) +measurement_update_code_generator.print_string("Equations for NE velocity innovation variance inverse") +measurement_update_code_generator.write_matrix(Matrix(S_inv_s), "_ekf_gsf[model_index].S_inverse", True) +measurement_update_code_generator.print_string("Equations for NE velocity Kalman gain") +measurement_update_code_generator.write_matrix(Matrix(K_s), "K", False) +measurement_update_code_generator.print_string("Equations for covariance matrix update") +measurement_update_code_generator.write_matrix(Matrix(P_new_s), "_ekf_gsf[model_index].P", True) +measurement_update_code_generator.close()