mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 01:54:08 +08:00
EKF: Convert EKF-GSF yaw estimator from matlab to sympy derived equations (#871)
* EKF: Add sympy derivation of equations for EKF-GSF yaw estimator * EKF: Convert EKF-GSF yaw estimator to use sympy generated equations * EKF: Add status prints for GSF-EKF yaw estimator cvode gen * EKF: Move generated code for GSF-EKF yaw estinmator to generated directory * Make it run with python3 * Cleanup yaw estimator code generation * Improve and simplify gsf measurement update code generation * Use improved auto generated measurement update code & remove obsolete updateInnovCovMatInv function * Delete generated ekf_derivation for the moment Co-authored-by: kamilritz <kritz@ethz.ch>
This commit is contained in:
Paul Riseborough
GitHub
parent
9d7d502c56
commit
6feb28c27d
@ -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<float, 2> 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<float, 3, 2> 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<float, 3, 2> 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<float, 2> &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) {
|
||||
|
||||
@ -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<float, 2> &S);
|
||||
|
||||
};
|
||||
|
||||
62
EKF/python/gsf_ekf_yaw_estimator/code_gen.py
Normal file
62
EKF/python/gsf_ekf_yaw_estimator/code_gen.py
Normal file
@ -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()
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
112
EKF/python/gsf_ekf_yaw_estimator/main.py
Normal file
112
EKF/python/gsf_ekf_yaw_estimator/main.py
Normal file
@ -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()
|
||||
Loading…
x
Reference in New Issue
Block a user