通过局部O3优化提升ekf运行速度by65% (e4050973) · Commits · Wenxi XU / Mambo

include/ares/ekf/CMakeLists.txt

+10 −0

Original line number	Diff line number	Diff line
		@@ -8,3 +8,13 @@ file(GLOB EKF_SOURCES
		)

		zephyr_library_sources(${EKF_SOURCES})

		zephyr_library_compile_options(
		-O3
		-fno-strict-overflow
		-fno-common
		-ffunction-sections
		-fdata-sections
		-ffreestanding
		-fno-builtin
		)
		No newline at end of file

include/ares/ekf/QuaternionEKF.c

+44 −1

Original line number	Diff line number	Diff line
		@@ -73,6 +73,45 @@ void IMU_QuaternionEKF_Init(float *init_quaternion, float process_noise1, float
		QEKF_INS.g = PHY_G;
		#endif

		// Initialize other members of QEKF_INS
		QEKF_INS.hasStoredBias = FALSE;
		QEKF_INS.gyro_dt = 0.0f;
		QEKF_INS.accel_dt = 0.0f;
		memset(QEKF_INS.RawGyro, 0, sizeof(QEKF_INS.RawGyro));
		memset(QEKF_INS.Gyro, 0, sizeof(QEKF_INS.Gyro));
		memset(QEKF_INS.Accel, 0, sizeof(QEKF_INS.Accel)); // Will be set by LPF on first data too
		QEKF_INS.gyro_norm = 0.0f;
		QEKF_INS.accl_norm = 0.0f;
		QEKF_INS.StableFlag = 0; // FALSE
		memset(QEKF_INS.OrientationCosine, 0, sizeof(QEKF_INS.OrientationCosine));
		QEKF_INS.AdaptiveGainScale = 1.0f;

		// Initialize quaternion q and derived Euler angles
		memcpy(QEKF_INS.q, init_quaternion, sizeof(QEKF_INS.q));
		float norm_init_q = invSqrt(QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[1] * QEKF_INS.q[1] +
		QEKF_INS.q[2] * QEKF_INS.q[2] + QEKF_INS.q[3] * QEKF_INS.q[3]);
		for (int i = 0; i < 4; ++i) {
		QEKF_INS.q[i] *= norm_init_q;
		}

		QEKF_INS.Yaw =
		atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[3] + QEKF_INS.q[1] * QEKF_INS.q[2]),
		2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[1] * QEKF_INS.q[1]) -
		1.0f) *
		57.295779513f;
		QEKF_INS.Pitch =
		atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] + QEKF_INS.q[2] * QEKF_INS.q[3]),
		2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[3] * QEKF_INS.q[3]) -
		1.0f) *
		57.295779513f;
		QEKF_INS.Roll =
		asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) *
		57.295779513f;

		QEKF_INS.YawAngleLast = QEKF_INS.Yaw;
		QEKF_INS.YawRoundCount = 0;
		QEKF_INS.YawTotalAngle = QEKF_INS.Yaw;

		// 初始化矩阵维度信息
		Kalman_Filter_Init(&QEKF_INS.IMU_QuaternionEKF, 4, 0, 3);
		Matrix_Init(&QEKF_INS.ChiSquare, 1, 1, (float *)QEKF_INS.ChiSquare_Data);
		@@ -85,8 +124,9 @@ void IMU_QuaternionEKF_Init(float *init_quaternion, float process_noise1, float
		// QEKF_INS.GyroBias[0] = ...; (This depends on how bias is handled at startup)
		// Assuming hasStoredBias will be set if DTS provides bias, otherwise GyroBias starts at 0
		// or probed.
		if (!QEKF_INS.hasStoredBias) {
		memset(QEKF_INS.GyroBias, 0, sizeof(QEKF_INS.GyroBias));

		}
		// 自定义函数初始化,用于扩展或增加kf的基础功能
		QEKF_INS.IMU_QuaternionEKF.User_Func0_f = IMU_QuaternionEKF_Observe;
		// We don't probe bias now. (Comment seems outdated, bias is handled)
		@@ -259,6 +299,9 @@ void IMU_QuaternionEKF_Predict_Update(float gx, float gy, float gz, float gyro_d
		void IMU_QuaternionEKF_Measurement_Update(float gx_raw, float gy_raw, float gz_raw, float gyro_dt,
		float ax, float ay, float az, float accel_dt)
		{
		if (gyro_dt <= 0 \|\| accel_dt <= 0) {
		return;
		}
		// 0.5(Ohm-Ohm^bias)*deltaT,用于更新工作点处的状态转移F矩阵
		static float halfgxdt, halfgydt, halfgzdt;
		static float accelInvNorm;

include/ares/ekf/imu_task.c

+13 −13

Original line number	Diff line number	Diff line
		#include <zephyr/drivers/sensor.h>
		#include "ares/ekf/QuaternionEKF.h"
		#include "QuaternionEKF.h"
		#include "imu_task.h"
		#include "zephyr/devicetree.h"
		#include "zephyr/drivers/pid.h"
		#include "zephyr/kernel.h"
		#include <zephyr/devicetree.h>
		#include <zephyr/drivers/pid.h>
		#include <zephyr/kernel.h>
		#include <zephyr/drivers/flash.h>
		#include <zephyr/storage/flash_map.h>
		#include <zephyr/fs/nvs.h>
		#include "zephyr/sys/printk.h"
		#include "zephyr/sys/time_units.h"
		#include <zephyr/sys/time_units.h>
		#include <math.h>
		#include <stdbool.h>
		#include <stdint.h>
		@@ -108,7 +107,7 @@ static void IMU_Sensor_temp_control(INS_t *data)
		IMU_temp_pwm_set(data->accel_dev);

		if (current_temp >= 65) {
		LOG_WRN("Current Temp: %.2f, PWM: %d\n", (double)current_temp,
		LOG_WRN("Current Temp: %.2f, PWM: %d", (double)current_temp,
		(int)temp_pwm_output);
		}
		}
		@@ -197,11 +196,11 @@ static void InitQuaternion(const struct device accel_dev, const struct device
		QEKF_INS.GyroBias[X] = matrix1[2][0];
		QEKF_INS.GyroBias[Y] = matrix1[2][1];
		QEKF_INS.GyroBias[Z] = matrix1[2][2];
		printk("AccelBias: %f, %f, %f\n", (double)QEKF_INS.AccelBias[X],
		LOG_INF("AccelBias: %f, %f, %f", (double)QEKF_INS.AccelBias[X],
		(double)QEKF_INS.AccelBias[Y], (double)QEKF_INS.AccelBias[Z]);
		printk("AccelBeta: %f, %f, %f\n", (double)QEKF_INS.AccelBeta[X],
		LOG_INF("AccelBeta: %f, %f, %f", (double)QEKF_INS.AccelBeta[X],
		(double)QEKF_INS.AccelBeta[Y], (double)QEKF_INS.AccelBeta[Z]);
		printk("GyroBias: %f, %f, %f\n", (double)QEKF_INS.GyroBias[X],
		LOG_INF("GyroBias: %f, %f, %f", (double)QEKF_INS.GyroBias[X],
		(double)QEKF_INS.GyroBias[Y], (double)QEKF_INS.GyroBias[Z]);
		QEKF_INS.hasStoredBias = true;
		} else {
		@@ -248,8 +247,9 @@ static void InitQuaternion(const struct device accel_dev, const struct device
		init_q4[i + 1] =
		axis_rot[i] * sinf(angle / 2.0f); // 轴角公式,第三轴为0(没有z轴分量)
		}
		// printk("Init Quaternion: %f, %f, %f, %f\n", init_q4[0], init_q4[1], init_q4[2],
		// init_q4[3]); printk("Accel: %f, %f, %f\n", acc_init[X], acc_init[Y], acc_init[Z]);
		LOG_INF("Init Quaternion: %f, %f, %f, %f", (double)init_q4[0], (double)init_q4[1],
		(double)init_q4[2], (double)init_q4[3]);
		LOG_INF("Accel: %f, %f, %f", (double)acc_init[X], (double)acc_init[Y], (double)acc_init[Z]);
		}

		static void IMU_Sensor_trig_handler(const struct device dev, const struct sensor_trigger trigger)
		@@ -316,7 +316,7 @@ void IMU_Sensor_trig_init(const struct device accel_dev, const struct device g
		pid_reg_time(temp_pwm_pid, &INS.accel_curr_cyc, &INS.accel_prev_cyc);

		if (!pwm_is_ready_dt(&pwm)) {
		printk("Error: PWM device %s is not ready\n", pwm.dev->name);
		LOG_INF("Error: PWM device %s is not ready", pwm.dev->name);
		return;
		}
		#endif // CONFIG_IMU_PWM_TEMP_CTRL

include/ares/ekf/kalman_filter.c

+30 −0

Original line number	Diff line number	Diff line
		@@ -155,6 +155,7 @@ void Kalman_Filter_Init(KalmanFilter_t *kf, uint8_t xhatSize, uint8_t uSize, uin
		kf->zSize = zSize;

		kf->MeasurementValidNum = 0;
		kf->UseAutoAdjustment = 0; // 初始化自动调整标志

		// measurement flags
		kf->MeasurementMap = (uint8_t )user_malloc(sizeof(uint8_t) zSize);
		@@ -191,6 +192,11 @@ void Kalman_Filter_Init(KalmanFilter_t *kf, uint8_t xhatSize, uint8_t uSize, uin
		kf->u_data = (float )user_malloc(sizeof_float uSize);
		memset(kf->u_data, 0, sizeof_float * uSize);
		Matrix_Init(&kf->u, kf->uSize, 1, (float *)kf->u_data);
		} else {
		kf->u_data = NULL;
		kf->u.pData = NULL;
		kf->u.numRows = 0;
		kf->u.numCols = 0;
		}

		// measurement vector z
		@@ -221,6 +227,11 @@ void Kalman_Filter_Init(KalmanFilter_t *kf, uint8_t xhatSize, uint8_t uSize, uin
		kf->B_data = (float )user_malloc(sizeof_float xhatSize * uSize);
		memset(kf->B_data, 0, sizeof_float * xhatSize * uSize);
		Matrix_Init(&kf->B, kf->xhatSize, kf->uSize, (float *)kf->B_data);
		} else {
		kf->B_data = NULL;
		kf->B.pData = NULL;
		kf->B.numRows = 0;
		kf->B.numCols = 0;
		}

		// measurement matrix H
		@@ -246,22 +257,41 @@ void Kalman_Filter_Init(KalmanFilter_t *kf, uint8_t xhatSize, uint8_t uSize, uin
		memset(kf->K_data, 0, sizeof_float * xhatSize * zSize);
		Matrix_Init(&kf->K, kf->xhatSize, kf->zSize, (float *)kf->K_data);

		// 临时矩阵和向量
		kf->S_data = (float )user_malloc(sizeof_float kf->xhatSize * kf->xhatSize);
		kf->temp_matrix_data = (float )user_malloc(sizeof_float kf->xhatSize * kf->xhatSize);
		kf->temp_matrix_data1 = (float )user_malloc(sizeof_float kf->xhatSize * kf->xhatSize);
		kf->temp_vector_data = (float )user_malloc(sizeof_float kf->xhatSize);
		kf->temp_vector_data1 = (float )user_malloc(sizeof_float kf->xhatSize);
		memset(kf->S_data, 0, sizeof_float * kf->xhatSize * kf->xhatSize);
		memset(kf->temp_matrix_data, 0, sizeof_float * kf->xhatSize * kf->xhatSize);
		memset(kf->temp_matrix_data1, 0, sizeof_float * kf->xhatSize * kf->xhatSize);
		memset(kf->temp_vector_data, 0, sizeof_float * kf->xhatSize);
		memset(kf->temp_vector_data1, 0, sizeof_float * kf->xhatSize);
		Matrix_Init(&kf->S, kf->xhatSize, kf->xhatSize, (float *)kf->S_data);
		Matrix_Init(&kf->temp_matrix, kf->xhatSize, kf->xhatSize, (float *)kf->temp_matrix_data);
		Matrix_Init(&kf->temp_matrix1, kf->xhatSize, kf->xhatSize, (float *)kf->temp_matrix_data1);
		Matrix_Init(&kf->temp_vector, kf->xhatSize, 1, (float *)kf->temp_vector_data);
		Matrix_Init(&kf->temp_vector1, kf->xhatSize, 1, (float *)kf->temp_vector_data1);

		// 初始化用户函数指针
		kf->User_Func0_f = NULL;
		kf->User_Func1_f = NULL;
		kf->User_Func2_f = NULL;
		kf->User_Func3_f = NULL;
		kf->User_Func4_f = NULL;
		kf->User_Func5_f = NULL;
		kf->User_Func6_f = NULL;

		// 初始化跳过标志
		kf->SkipEq1 = 0;
		kf->SkipEq2 = 0;
		kf->SkipEq3 = 0;
		kf->SkipEq4 = 0;
		kf->SkipEq5 = 0;

		// 初始化矩阵状态
		kf->MatStatus = 0;
		}

		void Kalman_Filter_Measure(KalmanFilter_t *kf)

Admin message