由于昨天测试了一些，程序写完之后跳动很大，所以借助网上的一些文章结合AI，给原来的数据读取进行了卡尔曼滤波，但是这一板有好多问题，比经我卡尔曼滤波也是第一次接触，做到不是很好
现在的主要问题是角度反馈有些慢，我打算后续加上FreeRTOS和DMA再看看
我就不给大家讲卡尔曼滤波了，个人认为学的也不通透，所以直接奉上代码，希望大家可以查证、
cpp
//这个是。c文件
#include <string.h>
#include <math.h>
#include "KALMAN.h"
// 状态向量：[q0, q1, q2, q3, bgx, bgy, bgz]
float state[7];
// 状态协方差矩阵 P (7x7)
float P[49];
// 过程噪声协方差矩阵 Q (7x7)
float Q[49];
// 观测噪声协方差矩阵 R (6x6)：3个加速度计和3个磁力计
float R[36];
// 状态转换矩阵 F (7x7)
float F[49];
// 观测矩阵 H (6x7)
float H[42];
// 卡尔曼增益 K (7x6)
float K[42];
// 预测状态
float state_pred[7];
// 预测协方差
float P_pred[49];
// 残差
float y[6];
// S = H*P*H^T + R
float S[36];
// 临时矩阵
float temp_matrix[49];
float S_inv[36];
// 过滤后的输出数据
float filtered_gyro[3]; // 修正后的角速度
float filtered_acc[3]; // 修正后的加速度
float filtered_mag[3]; // 修正后的磁场
// 四元数乘法
void quaternion_multiply(float *q1, float *q2, float *result) {
result[0] = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3];
result[1] = q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2];
result[2] = q1[0]*q2[2] - q1[1]*q2[3] + q1[2]*q2[0] + q1[3]*q2[1];
result[3] = q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0];
}
// 四元数归一化
void quaternion_normalize(float *q) {
float norm = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
if (norm > 1e-6f) {
q[0] /= norm;
q[1] /= norm;
q[2] /= norm;
q[3] /= norm;
}
}
// 四元数共轭
void quaternion_conjugate(float *q, float *result) {
result[0] = q[0];
result[1] = -q[1];
result[2] = -q[2];
result[3] = -q[3];
}
// 用四元数旋转向量
void quaternion_rotate_vector(float *q, float *v, float *result) {
float q_vec[4] = {0, v[0], v[1], v[2]};
float q_conj[4];
quaternion_conjugate(q, q_conj);
float temp[4];
quaternion_multiply(q, q_vec, temp);
quaternion_multiply(temp, q_conj, temp);
result[0] = temp[1] / temp[0];
result[1] = temp[2] / temp[0];
result[2] = temp[3] / temp[0];
}
// 矩阵乘法 C = A * B
void matrix_multiply(float *A, float *B, float *C, int A_rows, int A_cols, int B_cols) {
memset(C, 0, A_rows * B_cols * sizeof(float));
for (int i = 0; i < A_rows; i++) {
for (int j = 0; j < B_cols; j++) {
for (int k = 0; k < A_cols; k++) {
C[i * B_cols + j] += A[i * A_cols + k] * B[k * B_cols + j];
}
}
}
}
// 矩阵转置 B = A^T
void matrix_transpose(float *A, float *B, int rows, int cols) {
for (int i = 0; i < rows; i++) {
for (int j = 0; j < cols; j++) {
B[j * rows + i] = A[i * cols + j];
}
}
}
// 3x3矩阵求逆 (使用伴随矩阵法)
void matrix_inv_3x3(float *A, float *A_inv) {
float det = A[0] * (A[4] * A[8] - A[5] * A[7])
- A[1] * (A[3] * A[8] - A[5] * A[6])
+ A[2] * (A[3] * A[7] - A[4] * A[6]);
if (fabsf(det) < 1e-6f) {
memset(A_inv, 0, 9 * sizeof(float));
return;
}
float inv_det = 1.0f / det;
A_inv[0] = (A[4] * A[8] - A[5] * A[7]) * inv_det;
A_inv[1] = (A[2] * A[7] - A[1] * A[8]) * inv_det;
A_inv[2] = (A[1] * A[5] - A[2] * A[4]) * inv_det;
A_inv[3] = (A[5] * A[6] - A[3] * A[8]) * inv_det;
A_inv[4] = (A[0] * A[8] - A[2] * A[6]) * inv_det;
A_inv[5] = (A[2] * A[3] - A[0] * A[5]) * inv_det;
A_inv[6] = (A[3] * A[7] - A[4] * A[6]) * inv_det;
A_inv[7] = (A[1] * A[6] - A[0] * A[7]) * inv_det;
A_inv[8] = (A[0] * A[4] - A[1] * A[3]) * inv_det;
}
// 6x6矩阵求逆 (使用分块法)
int matrix_inv_6x6(float *A, float *A_inv) {
// 使用高斯消元法求逆
float temp[72]; // 6x12 临时矩阵 [A|I]
// 复制A到左边，单位矩阵到右边
memset(temp, 0, 72 * sizeof(float));
for (int i = 0; i < 6; i++) {
for (int j = 0; j < 6; j++) {
temp[i * 12 + j] = A[i * 6 + j];
}
temp[i * 12 + 6 + i] = 1.0f;
}
// 高斯消元
for (int col = 0; col < 6; col++) {
// 找主元
int pivot_row = col;
float max_val = fabsf(temp[col * 12 + col]);
for (int row = col + 1; row < 6; row++) {
float val = fabsf(temp[row * 12 + col]);
if (val > max_val) {
max_val = val;
pivot_row = row;
}
}
if (max_val < 1e-6f) return -1; // 奇异矩阵
// 交换行
if (pivot_row != col) {
for (int j = 0; j < 12; j++) {
float tmp = temp[col * 12 + j];
temp[col * 12 + j] = temp[pivot_row * 12 + j];
temp[pivot_row * 12 + j] = tmp;
}
}
// 归一化主行
float pivot = temp[col * 12 + col];
for (int j = 0; j < 12; j++) {
temp[col * 12 + j] /= pivot;
}
// 消元
for (int row = 0; row < 6; row++) {
if (row != col) {
float factor = temp[row * 12 + col];
for (int j = 0; j < 12; j++) {
temp[row * 12 + j] -= factor * temp[col * 12 + j];
}
}
}
}
// 提取逆矩阵
for (int i = 0; i < 6; i++) {
for (int j = 0; j < 6; j++) {
A_inv[i * 6 + j] = temp[i * 12 + 6 + j];
}
}
return 0;
}
// 矩阵减法 C = A - B
void matrix_subtract(float *A, float *B, float *C, int rows, int cols) {
for (int i = 0; i < rows * cols; i++) {
C[i] = A[i] - B[i];
}
}
// 矩阵加法 C = A + B
void matrix_add(float *A, float *B, float *C, int rows, int cols) {
for (int i = 0; i < rows * cols; i++) {
C[i] = A[i] + B[i];
}
}
// 矩阵标量乘法 A = A * scalar
void matrix_scale(float *A, float scalar, int rows, int cols) {
for (int i = 0; i < rows * cols; i++) {
A[i] *= scalar;
}
}
// 初始状态
void init_state(float initial_q[4], float initial_bg[3]) {
// 初始化状态向量
memcpy(state, initial_q, 4 * sizeof(float));
memcpy(state + 4, initial_bg, 3 * sizeof(float));
// 初始化协方差矩阵 P (对角矩阵)
memset(P, 0, 49 * sizeof(float));
P[0] = 0.01f; P[8] = 0.01f; P[16] = 0.01f; P[24] = 0.01f; // q的方差
P[32] = 0.01f; P[40] = 0.01f; P[48] = 0.01f; // bg的方差
// 初始化过程噪声协方差 Q
memset(Q, 0, 49 * sizeof(float));
Q[0] = 0.001f; Q[8] = 0.001f; Q[16] = 0.001f; Q[24] = 0.001f; // q的噪声
Q[32] = 0.0001f; Q[40] = 0.0001f; Q[48] = 0.0001f; // bg的噪声
// 初始化观测噪声协方差 R (对角矩阵)
memset(R, 0, 36 * sizeof(float));
R[0] = 0.1f; R[7] = 0.1f; R[14] = 0.1f; // 加速度计噪声
R[21] = 0.1f; R[28] = 0.1f; R[35] = 0.1f; // 磁力计噪声
}
// 计算状态转移矩阵F (雅可比矩阵)
void compute_F_matrix(float *q, float *gyro, float dt) {
// F = I + dt * Jf
memset(F, 0, 49 * sizeof(float));
// 单位矩阵
for (int i = 0; i < 7; i++) {
F[i * 7 + i] = 1.0f;
}
// 四元数动力学雅可比 (左上4x4块)
// dq/dt = 0.5 * q * omega
float half_dt = 0.5f * dt;
F[0] = 1.0f;
F[1] = -half_dt * gyro[0];
F[2] = -half_dt * gyro[1];
F[3] = -half_dt * gyro[2];
F[7] = half_dt * gyro[0];
F[8] = 1.0f;
F[10] = half_dt * gyro[2];
F[11] = -half_dt * gyro[1];
F[14] = half_dt * gyro[1];
F[17] = 1.0f;
F[18] = -half_dt * gyro[2];
F[19] = half_dt * gyro[0];
F[21] = half_dt * gyro[2];
F[24] = half_dt * gyro[1];
F[25] = 1.0f;
F[26] = -half_dt * gyro[0];
// 四元数对陀螺仪偏置的导数 (左上4x3块)
F[4] = -half_dt;
F[11] = -half_dt;
F[18] = -half_dt;
F[25] = -half_dt;
// 陀螺仪偏置保持不变 (右下3x3块)
F[32] = 1.0f; F[33] = 0; F[34] = 0;
F[39] = 0; F[40] = 1.0f; F[41] = 0;
F[46] = 0; F[47] = 0; F[48] = 1.0f;
}
// 计算观测矩阵H (雅可比矩阵)
void compute_H_matrix(float *q, float ref_mag[3]) {
memset(H, 0, 42 * sizeof(float));
float q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3];
// 加速度计观测对四元数的导数 (3x4块)
// 期望加速度是 [0, 0, 0, -1] 经过四元数旋转
H[0] = 2 * q2;
H[1] = 2 * q3;
H[2] = 2 * q0;
H[3] = 2 * q1;
H[7] = -2 * q1;
H[8] = 2 * q0;
H[9] = -2 * q3;
H[10] = 2 * q2;
H[14] = -2 * q0;
H[15] = -2 * q3;
H[16] = 2 * q2;
H[17] = 2 * q1;
// 磁力计观测对四元数的导数 (3x4块)
float mx = ref_mag[0], my = ref_mag[1], mz = ref_mag[2];
H[21] = 2 * (mx * q2 + my * q3 + mz * q0);
H[22] = 2 * (mx * q3 - my * q0 + mz * q1);
H[23] = 2 * (-mx * q0 + my * q1 + mz * q2);
H[24] = 2 * (mx * q1 - my * q2 + mz * q3);
H[28] = 2 * (mx * q3 + my * q0 - mz * q1);
H[29] = 2 * (mx * q2 - my * q1 - mz * q0);
H[30] = 2 * (mx * q1 + my * q2 + mz * q3);
H[31] = 2 * (-mx * q0 - my * q3 + mz * q2);
H[35] = 2 * (-mx * q0 + my * q1 + mz * q2);
H[36] = 2 * (-mx * q1 - my * q2 + mz * q3);
H[37] = 2 * (mx * q2 - my * q3 - mz * q0);
H[38] = 2 * (mx * q3 + my * q2 + mz * q1);
}
// 预测步骤
void predict(float gyro[3], float dt) {
// 1. 预测状态
// 构造旋转四元数 dq = [cos(theta/2), sin(theta/2)*omega/|omega|]
// 单位转换：deg/s -> rad/s
float gyro_rad[3] = {
gyro[0] * 0.0174532925f, // * PI/180
gyro[1] * 0.0174532925f,
gyro[2] * 0.0174532925f
};
float omega_sq = gyro_rad[0]*gyro_rad[0] + gyro_rad[1]*gyro_rad[1] + gyro_rad[2]*gyro_rad[2];
float theta = sqrtf(omega_sq) * dt;
float dq[4];
if (theta > 1e-6f) {
float half_sin = sinf(theta / 2);
float inv_theta = half_sin / theta;
dq[0] = cosf(theta / 2);
dq[1] = gyro_rad[0] * inv_theta;
dq[2] = gyro_rad[1] * inv_theta;
dq[3] = gyro_rad[2] * inv_theta;
} else {
dq[0] = 1.0f;
dq[1] = 0;
dq[2] = 0;
dq[3] = 0;
}
// 四元数乘法更新姿态
float q_new[4];
quaternion_multiply(state, dq, q_new);
quaternion_normalize(q_new);
memcpy(state_pred, q_new, 4 * sizeof(float));
// 陀螺仪偏置保持不变
memcpy(state_pred + 4, state + 4, 3 * sizeof(float));
// 2. 计算状态转移矩阵 F
compute_F_matrix(state, gyro_rad, dt);
// 3. 预测协方差 P_pred = F * P * F^T + Q
float F_transpose[49];
matrix_transpose(F, F_transpose, 7, 7);
float temp[49];
matrix_multiply(F, P, temp, 7, 7, 7);
matrix_multiply(temp, F_transpose, P_pred, 7, 7, 7);
matrix_add(P_pred, Q, P_pred, 7, 7);
// 4. 更新状态和协方差
memcpy(state, state_pred, 7 * sizeof(float));
memcpy(P, P_pred, 49 * sizeof(float));
}
// 更新步骤
void update(float acc[3], float mag[3], float ref_mag[3]) {
// 1. 计算期望观测值
float q_conj[4];
quaternion_conjugate(state, q_conj);
// 期望加速度计测量 (重力向量 [0,0,0,-1])
float gravity[4] = {0, 0, 0, -1};
float expected_acc[4];
quaternion_multiply(state, gravity, expected_acc);
quaternion_multiply(expected_acc, q_conj, expected_acc);
float h_acc[3] = {
expected_acc[1] / expected_acc[0],
expected_acc[2] / expected_acc[0],
expected_acc[3] / expected_acc[0]
};
// 期望磁力计测量
float ref_mag_q[4] = {0, ref_mag[0], ref_mag[1], ref_mag[2]};
float expected_mag[4];
quaternion_multiply(state, ref_mag_q, expected_mag);
quaternion_multiply(expected_mag, q_conj, expected_mag);
float h_mag[3] = {
expected_mag[1] / expected_mag[0],
expected_mag[2] / expected_mag[0],
expected_mag[3] / expected_mag[0]
};
// 2. 计算残差 y = z - h
float z[6] = {acc[0], acc[1], acc[2], mag[0], mag[1], mag[2]};
float h[6] = {h_acc[0], h_acc[1], h_acc[2], h_mag[0], h_mag[1], h_mag[2]};
for (int i = 0; i < 6; i++) {
y[i] = z[i] - h[i];
}
// 3. 计算观测矩阵 H
compute_H_matrix(state, ref_mag);
// 4. 计算 S = H * P * H^T + R
float H_transpose[42];
matrix_transpose(H, H_transpose, 6, 7);
float temp1[36];
matrix_multiply(H, P, temp1, 6, 7, 7);
matrix_multiply(temp1, H_transpose, S, 6, 7, 6);
matrix_add(S, R, S, 6, 6);
// 5. 计算 S 的逆
if (matrix_inv_6x6(S, S_inv) != 0) {
return; // 矩阵奇异，跳过更新
}
// 6. 计算卡尔曼增益 K = P * H^T * S^-1
float temp2[42];
matrix_multiply(P, H_transpose, temp2, 7, 7, 6);
matrix_multiply(temp2, S_inv, K, 7, 6, 6);
// 7. 更新状态 x = x + K * y
for (int i = 0; i < 7; i++) {
float delta = 0;
for (int j = 0; j < 6; j++) {
delta += K[i * 6 + j] * y[j];
}
state[i] += delta;
}
// 8. 归一化四元数
quaternion_normalize(state);
// 9. 更新协方差 P = (I - K * H) * P_pred
float KH[49];
matrix_multiply(K, H, KH, 7, 6, 7);
float I_KH[49];
memset(I_KH, 0, 49 * sizeof(float));
for (int i = 0; i < 7; i++) {
I_KH[i * 7 + i] = 1.0f;
for (int j = 0; j < 7; j++) {
I_KH[i * 7 + j] -= KH[i * 7 + j];
}
}
matrix_multiply(I_KH, P_pred, P, 7, 7, 7);
}
// 获取过滤后的传感器数据
void get_filtered_data(float *out_gyro, float *out_acc, float *out_mag) {
// 修正后的角速度 = 原始角速度 - 估计的偏置
// 注意：这里保存的是最后一次 predict() 使用的原始陀螺仪数据
// 如果需要原始值，需要在 predict 中保存
// 修正后的加速度 = 基于估计姿态的重力补偿
float q_conj[4];
quaternion_conjugate(state, q_conj);
float gravity[4] = {0, 0, 0, -1};
float expected_gravity[4];
quaternion_multiply(state, gravity, expected_gravity);
quaternion_multiply(expected_gravity, q_conj, expected_gravity);
// 过滤后的加速度 = 原始加速度 - 重力分量
// 注意：这里需要传入原始加速度，或保存原始值
// 修正后的磁场 = 基于估计姿态的磁场
// 注意：这里需要传入原始磁场，或保存原始值
// 简化版本：直接返回当前估计的姿态对应的期望值
out_acc[0] = expected_gravity[1] / expected_gravity[0];
out_acc[1] = expected_gravity[2] / expected_gravity[0];
out_acc[2] = expected_gravity[3] / expected_gravity[0];
}
// 获取修正后的陀螺仪数据（扣除偏置）
// 输入和输出都是 deg/s
void get_filtered_gyro(float *raw_gyro, float *out_gyro) {
// state[4-6] 估计的偏置单位是 rad/s，需要转换为 deg/s
float bgx_deg = state[4] * 57.2957795f; // rad/s -> deg/s
float bgy_deg = state[5] * 57.2957795f;
float bgz_deg = state[6] * 57.2957795f;
out_gyro[0] = raw_gyro[0] - bgx_deg; // 扣除x轴偏置
out_gyro[1] = raw_gyro[1] - bgy_deg; // 扣除y轴偏置
out_gyro[2] = raw_gyro[2] - bgz_deg; // 扣除z轴偏置
}
// 获取重力补偿后的加速度
void get_filtered_acc(float *raw_acc, float *out_acc) {
float q_conj[4];
quaternion_conjugate(state, q_conj);
// 计算当前姿态下的重力向量
float gravity[4] = {0, 0, 0, -1}; // 世界坐标系重力向下
float expected_gravity[4];
quaternion_multiply(state, gravity, expected_gravity);
quaternion_multiply(expected_gravity, q_conj, expected_gravity);
float gx = expected_gravity[1] / expected_gravity[0];
float gy = expected_gravity[2] / expected_gravity[0];
float gz = expected_gravity[3] / expected_gravity[0];
// 过滤后的加速度 = 原始加速度 - 重力分量
out_acc[0] = raw_acc[0] - gx;
out_acc[1] = raw_acc[1] - gy;
out_acc[2] = raw_acc[2] - gz;
}
// 获取航向修正后的磁场
void get_filtered_mag(float *raw_mag, float *ref_mag, float *out_mag) {
float q_conj[4];
quaternion_conjugate(state, q_conj);
// 计算期望的磁场向量
float ref_mag_q[4] = {0, ref_mag[0], ref_mag[1], ref_mag[2]};
float expected_mag[4];
quaternion_multiply(state, ref_mag_q, expected_mag);
quaternion_multiply(expected_mag, q_conj, expected_mag);
out_mag[0] = expected_mag[1] / expected_mag[0];
out_mag[1] = expected_mag[2] / expected_mag[0];
out_mag[2] = expected_mag[3] / expected_mag[0];这个是.h文件
cpp
#ifndef KALMAN_H
#define KALMAN_H
#include <stdint.h>
// 全局状态向量：[q0, q1, q2, q3, bgx, bgy, bgz]
extern float state[7];
// 全局协方差矩阵
extern float P[49];
extern float Q[49];
extern float R[36];
extern float F[49];
extern float H[42];
extern float K[42];
extern float state_pred[7];
extern float P_pred[49];
extern float y[6];
extern float S[36];
// 过滤后的输出数据
extern float filtered_gyro[3]; // 修正后的角速度
extern float filtered_acc[3]; // 修正后的加速度
extern float filtered_mag[3]; // 修正后的磁场
// ========== 四元数运算函数 ==========
void quaternion_multiply(float *q1, float *q2, float *result);
void quaternion_normalize(float *q);
void quaternion_conjugate(float *q, float *result);
void quaternion_rotate_vector(float *q, float *v, float *result);
// ========== 矩阵运算函数 ==========
void matrix_multiply(float *A, float *B, float *C, int A_rows, int A_cols, int B_cols);
void matrix_transpose(float *A, float *B, int rows, int cols);
void matrix_inv_3x3(float *A, float *A_inv);
int matrix_inv_6x6(float *A, float *A_inv);
void matrix_subtract(float *A, float *B, float *C, int rows, int cols);
void matrix_add(float *A, float *B, float *C, int rows, int cols);
void matrix_scale(float *A, float scalar, int rows, int cols);
// ========== 卡尔曼滤波核心函数 ==========
void init_state(float initial_q[4], float initial_bg[3]);
void compute_F_matrix(float *q, float *gyro, float dt);
void compute_H_matrix(float *q, float ref_mag[3]);
void predict(float gyro[3], float dt);
void update(float acc[3], float mag[3], float ref_mag[3]);
// ========== 获取过滤后的传感器数据 ==========
// 获取修正后的陀螺仪（扣除估计的偏置）
void get_filtered_gyro(float *raw_gyro, float *out_gyro);
// 获取重力补偿后的加速度（去除重力分量）
void get_filtered_acc(float *raw_acc, float *out_acc);
// 获取航向修正后的磁场
void get_filtered_mag(float *raw_mag, float *ref_mag, float *out_mag);
#endif这两个分别是写的KALMAN.c和.h文件
我的这个程序只是对于咱们前面的IMU数据的读取
然后再main.c中怎么用在下面
先定义初始化的矩阵函数
cpp
float initial_q[4] = {1, 0, 0, 0};
quaternion_normalize(initial_q);
init_state(initial_q, (float[3]){0, 0, 0});
// 参考磁场（根据实际位置调整）
float ref_mag[3] = {0.65f, -0.057f, -0.76f};
float filtered_gyro[3], filtered_acc[3];然后用函数就行
cpp
float delay_time = 0.01f;
predict(gyro, 0.01f);
update(accel, mag, ref_mag);
get_filtered_gyro(gyro, filtered_gyro);
get_filtered_acc(accel, filtered_acc);