夜话卡尔曼滤波(3) - 请Deepseek帮帮忙-优快云博客

本文链接：https://blog.youkuaiyun.com/tenth_quadrant/article/details/146181268

1 序

前面用了两篇文章粗略地介绍了卡尔曼滤波相关公式，以及公式中相关的参数，并顺带介绍了里面有矩阵、向量概念，主要目的是为了把握主线，形成主体思想。具体编程可以有很多参考，但是我们需要具备一定的基础知识，不至于出现问题时茫然无措，寻遍四周无方可求，这个世界上，自驱力才是终极动力。
在写这个系列时，正好deepseek问世可用，不妨偷偷懒，用它来编一下程序，正好有个直觉的体会。声明一下，下面提到的AI生成代码的问题，只是当前这个时间有意义，毕竟这些工具迭代是很快的，在这里只是当下体会。

2 AI编程目标

我们仍以前面的二维定位小车定位为目标，测量向量有x/y两个轴向的坐标，状态量有x/y坐标以及这两个轴向的速度。现在假定采用采用stm32F103RC系列MCU来处理这个任务，要求刷新频率是100ms一次。
在stm32的编程环境中，有一个直接可以用的数学库，称为CMSIS-DSP，里面含有矩阵运算的功能，无论是从代码成熟度或者执行速度上看，应该比我们手搓的要好，所以编程中要用上它。

请帮我编一个程序，运行在stm32F103RC上面，实现卡尔曼滤波功能，
输入量是x、y坐标，将x、y坐标和这两个方向的速度作为状态量。
要求用标准库，并且调用CMSIS-DSP库。keil是5.23版本

上面的请求文本中没有加刷新频率，因为加上这个以后，会产生一堆与卡尔曼滤波无关的代码，所以暂时没加。

3 百度的满血版

打开百度首页，就有ds使用广告，正好先用一下。将这个请求输入进去，看看能不能解我们烦忧。中间思考过程就不摘录了，只摘录分析的结果。

/* 卡尔曼滤波头文件 Kalman.h */
#include "stm32f10x.h"
#include "arm_math.h"

#define STATE_DIM 4  // x,y,vx,vy
#define MEAS_DIM   2  // x,y

typedef struct {
    arm_matrix_instance_f32 F;   // 状态转移矩阵 4x4
    arm_matrix_instance_f32 H;   // 测量矩阵 2x4
    arm_matrix_instance_f32 Q;   // 过程噪声 4x4
    arm_matrix_instance_f32 R;   // 测量噪声 2x2
    arm_matrix_instance_f32 P;   // 误差协方差 4x4
    arm_matrix_instance_f32 x;   // 状态向量 4x1
} KalmanFilter;

void Kalman_Init(KalmanFilter *kf);
void Kalman_Predict(KalmanFilter *kf);
void Kalman_Update(KalmanFilter *kf, float *measurement);

/* 卡尔曼滤波实现文件 Kalman.c */
#include "Kalman.h"

// 临时矩阵存储
static float F_data = {1,0,0.1,0,
                           0,1,0,0.1,
                           0,0,1,0,
                           0,0,0,1};  // 假设采样周期0.1s

static float H_data‌:ml-citation{ref="5" data="citationList"} = {1,0,0,0,
                          0,1,0,0};

static float Q_data = {0.01,0,0,0,
                          0,0.01,0,0,
                          0,0,0.1,0,
                          0,0,0,0.1};

static float R_data‌:ml-citation{ref="7" data="citationList"} = {0.1,0,
                         0,0.1};

static float P_data = {1,0,0,0,
                          0,1,0,0,
                          0,0,1,0,
                          0,0,0,1};

static float x_data‌:ml-citation{ref="7" data="citationList"} = {0};

void Kalman_Init(KalmanFilter *kf) {
    // 初始化矩阵实例
    arm_mat_init_f32(&kf->F, STATE_DIM, STATE_DIM, F_data);
    arm_mat_init_f32(&kf->H, MEAS_DIM, STATE_DIM, H_data);
    arm_mat_init_f32(&kf->Q, STATE_DIM, STATE_DIM, Q_data);
    arm_mat_init_f32(&kf->R, MEAS_DIM, MEAS_DIM, R_data);
    arm_mat_init_f32(&kf->P, STATE_DIM, STATE_DIM, P_data);
    arm_mat_init_f32(&kf->x, STATE_DIM, 1, x_data);
}

void Kalman_Predict(KalmanFilter *kf) {
    // 状态预测: x = F * x
    arm_matrix_instance_f32 x_pred;
    float x_pred_data[STATE_DIM];
    arm_mat_init_f32(&x_pred, STATE_DIM, 1, x_pred_data);
    arm_mat_mult_f32(&kf->F, &kf->x, &x_pred);
    arm_copy_f32(x_pred_data, kf->x.pData, STATE_DIM);

    // 协方差预测: P = F * P * F^T + Q
    arm_matrix_instance_f32 FP, FPFt;
    float FP_data[STATE_DIM*STATE_DIM], FPFt_data[STATE_DIM*STATE_DIM];
    arm_mat_init_f32(&FP, STATE_DIM, STATE_DIM, FP_data);
    arm_mat_init_f32(&FPFt, STATE_DIM, STATE_DIM, FPFt_data);
    
    arm_mat_mult_f32(&kf->F, &kf->P, &FP);
    arm_mat_trans_f32(&kf->F, &kf->F);
    arm_mat_mult_f32(&FP, &kf->F, &FPFt);
    arm_mat_add_f32(&FPFt, &kf->Q, &kf->P);
}

void Kalman_Update(KalmanFilter *kf, float *measurement) {
    // 计算卡尔曼增益
    arm_matrix_instance_f32 Ht, PHt, S, K;
    float Ht_data[STATE_DIM*MEAS_DIM], PHt_data[STATE_DIM*MEAS_DIM];
    float S_data[MEAS_DIM*MEAS_DIM], K_data[STATE_DIM*MEAS_DIM];
    
    arm_mat_init_f32(&Ht, STATE_DIM, MEAS_DIM, Ht_data);
    arm_mat_init_f32(&PHt, STATE_DIM, MEAS_DIM, PHt_data);
    arm_mat_init_f32(&S, MEAS_DIM, MEAS_DIM, S_data);
    arm_mat_init_f32(&K, STATE_DIM, MEAS_DIM, K_data);

    arm_mat_trans_f32(&kf->H, &Ht);
    arm_mat_mult_f32(&kf->P, &Ht, &PHt);
    arm_mat_mult_f32(&kf->H, &PHt, &S);
    arm_mat_add_f32(&S, &kf->R, &S);
    arm_mat_inverse_f32(&S, &S);  // 需要CMSIS-DSP支持
    
    arm_mat_mult_f32(&PHt, &S, &K);

    // 状态更新
    arm_matrix_instance_f32 y, z;
    float y_data[MEAS_DIM], z_data[MEAS_DIM];
    arm_mat_init_f32(&y, MEAS_DIM, 1, measurement);
    arm_mat_init_f32(&z, MEAS_DIM, 1, z_data);
    
    arm_mat_mult_f32(&kf->H, &kf->x, &z);
    arm_mat_sub_f32(&y, &z, &y);
    
    arm_matrix_instance_f32 Ky;
    float Ky_data[STATE_DIM];
    arm_mat_init_f32(&Ky, STATE_DIM, 1, Ky_data);
    arm_mat_mult_f32(&K, &y, &Ky);
    
    arm_mat_add_f32(&kf->x, &Ky, &kf->x);

    // 协方差更新
    arm_matrix_instance_f32 I, KH;
    float I_data[STATE_DIM*STATE_DIM], KH_data[STATE_DIM*STATE_DIM];
    arm_mat_init_f32(&I, STATE_DIM, STATE_DIM, I_data);
    arm_mat_init_f32(&KH, STATE_DIM, STATE_DIM, KH_data);
    
    arm_mat_scale_f32(&K, 1.0f, &KH);
    arm_mat_mult_f32(&KH, &kf->H, &KH);
    arm_mat_sub_f32(&I, &KH, &I);
    arm_mat_mult_f32(&I, &kf->P, &kf->P);
}

/* 主程序示例 main.c */
#include "Kalman.h"

KalmanFilter kf;

int main(void) {
    // 初始化硬件
    SystemInit();
    // 初始化CMSIS-DSP库
    arm_status status = arm_mat_init_f32(NULL, 0, 0, NULL);
    
    // 初始化卡尔曼滤波器
    Kalman_Init(&kf);
    
    while(1) {
        // 获取测量值（示例数据）
        float measurement‌:ml-citation{ref="2" data="citationList"} = {get_x(), get_y()};  // 需实现传感器读取
        
        // 执行预测步骤
        Kalman_Predict(&kf);
        
        // 执行更新步骤
        Kalman_Update(&kf, measurement);
        
        // 获取状态估计结果
        float x = kf.x.pData;
        float y = kf.x.pData‌:ml-citation{ref="1" data="citationList"};
        float vx = kf.x.pData‌:ml-citation{ref="2" data="citationList"};
        float vy = kf.x.pData‌:ml-citation{ref="3" data="citationList"};
        
        // 输出结果或进行其他处理
        delay_ms(10);  // 假设采样周期10ms
    }
}

可以看出，整体架子是扎起来，能不能用？毛毛地看一下，有语法错误。补充请求：

你这个代码中类似于“:ml-citation{ref=””这样的代码是错误的，请改正，需要可执行的代码，拜托走点心！

基本上该有什么问题还是有什么问题，比如R矩阵：

// 测量噪声矩阵
static float R_data‌:ml-citation{ref="1" data="citationList"} = {
    0.1, 0,
    0, 0.1
};

只是多了一行注释，但是毛病就是不改。再来一次：

请帮我编一个程序，运行在stm32F103RC上面，实现卡尔曼滤波功能，
输入量是x、y坐标，将x、y坐标和这两个方向的速度作为状态量。
要求用标准库，并且调用CMSIS-DSP库。
keil是5.23版本。要求代码可以编译。
-----------------------------------------------------------
好的，我现在需要帮用户编写一个在STM32F103RC上运行的卡尔曼滤波程序，
使用标准库和CMSIS-DSP库，并且确保代码能编译通过。用户之前提供的代码
存在一些错误，比如矩阵声明的语法问题，我需要先修正这些错误。

/* Kalman.c */
#include "Kalman.h"

// 状态转移矩阵（dt需在初始化时设置）
static float F_data;  
// 测量矩阵
static float H_data‌:ml-citation{ref="5" data="citationList"} = {1,0,0,0, 0,1,0,0};  
// 过程噪声矩阵
static float Q_data = {0.01,0,0,0, 0,0.01,0,0, 0,0,0.1,0, 0,0,0,0.1};  
// 测量噪声矩阵
static float R_data‌:ml-citation{ref="7" data="citationList"} = {0.1,0, 0,0.1};  
// 初始协方差矩阵
static float P_data = {1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1};  
// 初始状态向量
static float x_data‌:ml-citation{ref="7" data="citationList"} = {0};  
。。。。。。。

问题依旧存在，真是死性不改，如果没有改动代码的能力，那么这些生成的代码只具有一些参考意义，不能拿来直接用。
结论: 失败。

4 deepseek官网

百度的版本失败了，不知是不是没有掏银子的原因。现在再来试一下ds的官网。
官网产生的代码至少有一些类似于markdown似的小窗口，给人感觉很专业的样子。
在这里插入图片描述

看看代码：

#include "stm32f10x.h"
#include "arm_math.h"

// 状态维度4(x, vx, y, vy)，测量维度2(x, y)
#define STATE_DIM 4
#define MEAS_DIM 2

// 卡尔曼滤波器结构体
typedef struct {
    arm_matrix_instance_f32 F;   // 状态转移矩阵
    arm_matrix_instance_f32 H;   // 测量矩阵
    arm_matrix_instance_f32 Q;   // 过程噪声协方差
    arm_matrix_instance_f32 R;   // 测量噪声协方差
    arm_matrix_instance_f32 P;   // 估计误差协方差
    arm_matrix_instance_f32 x;   // 状态向量
} KalmanFilter;

// 矩阵和向量存储空间
static float F_data[STATE_DIM*STATE_DIM];
static float H_data[MEAS_DIM*STATE_DIM];
static float Q_data[STATE_DIM*STATE_DIM];
static float R_data[MEAS_DIM*MEAS_DIM];
static float P_data[STATE_DIM*STATE_DIM];
static float x_data[STATE_DIM];

// 临时矩阵存储
static float tmp44_1[STATE_DIM*STATE_DIM], tmp44_2[STATE_DIM*STATE_DIM];
static float tmp42_1[STATE_DIM*MEAS_DIM], tmp24_1[MEAS_DIM*STATE_DIM];
static float tmp22_1[MEAS_DIM*MEAS_DIM], tmp22_2[MEAS_DIM*MEAS_DIM];
static float K_data[STATE_DIM*MEAS_DIM]; // 卡尔曼增益

// 初始化卡尔曼滤波器
void Kalman_Init(KalmanFilter* kf, float dt) 
{
    // 状态转移矩阵F (匀速模型)
    memset(F_data, 0, sizeof(F_data));
    F_data[0] = 1; F_data[1] = dt;  // x
    F_data[4] = 0; F_data[5] = 1;   // vx
    F_data[10] = 1; F_data[11] = dt;// y
    F_data[14] = 0; F_data[15] = 1; // vy
    arm_mat_init_f32(&kf->F, STATE_DIM, STATE_DIM, F_data);

    // 测量矩阵H
    memset(H_data, 0, sizeof(H_data));
    H_data[0] = 1; // 测量x
    H_data[6] = 1; // 测量y
    arm_mat_init_f32(&kf->H, MEAS_DIM, STATE_DIM, H_data);

    // 过程噪声协方差Q (假设加速度噪声为0.5)
    float q = 0.5 * dt * dt;
    memset(Q_data, 0, sizeof(Q_data));
    Q_data[0] = q*q;  Q_data[1] = q*dt;
    Q_data[4] = q*dt; Q_data[5] = dt*dt;
    Q_data[10] = q*q; Q_data[11] = q*dt;
    Q_data[14] = q*dt;Q_data[15] = dt*dt;
    arm_mat_init_f32(&kf->Q, STATE_DIM, STATE_DIM, Q_data);

    // 测量噪声协方差R (假设测量噪声方差为0.1)
    memset(R_data, 0, sizeof(R_data));
    R_data[0] = R_data[3] = 0.1;
    arm_mat_init_f32(&kf->R, MEAS_DIM, MEAS_DIM, R_data);

    // 初始估计误差协方差P
    memset(P_data, 0, sizeof(P_data));
    P_data[0] = P_data[5] = P_data[10] = P_data[15] = 1.0;
    arm_mat_init_f32(&kf->P, STATE_DIM, STATE_DIM, P_data);

    // 初始状态向量
    memset(x_data, 0, sizeof(x_data));
    arm_mat_init_f32(&kf->x, STATE_DIM, 1, x_data);
}

// 卡尔曼预测
void Kalman_Predict(KalmanFilter* kf)
{
    // 状态预测: x = F * x
    arm_mat_mult_f32(&kf->F, &kf->x, &kf->x);
    
    // 协方差预测: P = F * P * F^T + Q
    arm_mat_mult_f32(&kf->F, &kf->P, (arm_matrix_instance_f32*)&tmp44_1);
    arm_mat_trans_f32(&kf->F, (arm_matrix_instance_f32*)&tmp44_2);
    arm_mat_mult_f32((arm_matrix_instance_f32*)&tmp44_1, 
                    (arm_matrix_instance_f32*)&tmp44_2, 
                    &kf->P);
    arm_mat_add_f32(&kf->P, &kf->Q, &kf->P);
}

// 卡尔曼更新
void Kalman_Update(KalmanFilter* kf, float* measurement)
{
    arm_matrix_instance_f32 z;
    float z_data[MEAS_DIM] = {measurement[0], measurement[1]};
    arm_mat_init_f32(&z, MEAS_DIM, 1, z_data);
    
    // 计算卡尔曼增益K = P * H^T * (H * P * H^T + R)^-1
    arm_mat_trans_f32(&kf->H, (arm_matrix_instance_f32*)&tmp24_1);
    arm_mat_mult_f32(&kf->P, (arm_matrix_instance_f32*)&tmp24_1, 
                    (arm_matrix_instance_f32*)&tmp42_1);
    arm_mat_mult_f32(&kf->H, &kf->P, (arm_matrix_instance_f32*)&tmp24_1);
    arm_mat_mult_f32((arm_matrix_instance_f32*)&tmp24_1, &kf->H, 
                    (arm_matrix_instance_f32*)&tmp22_1);
    arm_mat_add_f32((arm_matrix_instance_f32*)&tmp22_1, &kf->R, 
                    (arm_matrix_instance_f32*)&tmp22_1);
    arm_mat_inverse_f32((arm_matrix_instance_f32*)&tmp22_1, 
                       (arm_matrix_instance_f32*)&tmp22_2);
    arm_mat_mult_f32((arm_matrix_instance_f32*)&tmp42_1, 
                    (arm_matrix_instance_f32*)&tmp22_2, 
                    (arm_matrix_instance_f32*)K_data);
    
    // 更新状态估计: x = x + K * (z - H * x)
    arm_mat_mult_f32(&kf->H, &kf->x, (arm_matrix_instance_f32*)&tmp22_1);
    arm_mat_sub_f32(&z, (arm_matrix_instance_f32*)&tmp22_1, 
                   (arm_matrix_instance_f32*)&tmp22_1);
    arm_mat_mult_f32((arm_matrix_instance_f32*)K_data, 
                    (arm_matrix_instance_f32*)&tmp22_1, &kf->x);
    
    // 更新协方差估计: P = (I - K * H) * P
    arm_mat_mult_f32((arm_matrix_instance_f32*)K_data, &kf->H, 
                    (arm_matrix_instance_f32*)&tmp44_1);
    arm_mat_scale_f32((arm_matrix_instance_f32*)&tmp44_1, -1, 
                     (arm_matrix_instance_f32*)&tmp44_1);
    arm_mat_add_f32((arm_matrix_instance_f32*)&tmp44_1, &kf->P, &kf->P);
}

int main(void)
{
    // 初始化系统时钟等
    SystemInit();
    
    // 初始化卡尔曼滤波器，假设采样时间0.1秒
    KalmanFilter kf;
    Kalman_Init(&kf, 0.1f);
    
    // 模拟测量数据
    float measurements[][2] = {{1.1, 2.0}, {1.2, 2.3}, {1.3, 2.6}};
    
    for(int i=0; i<3; i++) {
        Kalman_Predict(&kf);
        Kalman_Update(&kf, measurements[i]);
        
        // 此处可以获取处理后的状态值：
        // x_data[0] - x位置
        // x_data[1] - x速度
        // x_data[2] - y位置
        // x_data[3] - y速度
    }
    
    while(1);
}

这个靠谱！可以直接编译！并且它的运算结果也是对的。
尤其是赞的是下面的代码：

    // 过程噪声协方差Q (假设加速度噪声为0.5)
    float q = 0.5 * dt * dt;
    memset(Q_data, 0, sizeof(Q_data));
    Q_data[0] = q*q;  Q_data[1] = q*dt;
    Q_data[4] = q*dt; Q_data[5] = dt*dt;
    Q_data[10] = q*q; Q_data[11] = q*dt;
    Q_data[14] = q*dt;Q_data[15] = dt*dt;
    arm_mat_init_f32(&kf->Q, STATE_DIM, STATE_DIM, Q_data);

这个整得明明白白的，非常直观易懂。
结论: 成功。

5 整理提问

对于代码生成式请求，对Deepseek要求不要过于笼统，下面把问题细化一下:

请帮我编一个运行在stm32F103RC上面卡尔曼滤波功能程序。

概况

- 具体场景：小车的平面位置估计
- 输入数据：小车的x/y两个轴向的坐标(浮点数)，即观测量：[x坐标, y坐标]
- 状态量：[x, y, vx, vy] 小车基本可以假定匀速状态，加速度很小，不用考虑
- 噪声参数：传感器测量误差为50cm
- 刷新频率：10Hz
- 期望输出：滤波后的x/y位置，需要实时性强


硬件细节
- 具体型号：STM32F103RC
- 外设接口：传感器通过UART1/UART2上传x/y坐标
- 资源限制：72MHz, 256kB Flash, 48kB SRAM,需要低功耗

软件环境：
- 库版本：采用标准库，V3.5.0 ,采用CMSIS-DSP库
- 浮点处理：如果可能，启用硬件浮点FP。
- 无实时操作系统
- Keil编译环境 5.23版本


代码约束：
- 浮点：如果无FPU时，采用固定点运算，以加快速度
- 内存：不采用动态分配，全部采有静态量

根据需求分析，提供以下优化实现的卡尔曼滤波代码方案（基于CMSIS-DSP的定点运算版本）：

/* 文件：kalman_filter.h */
#ifndef __KALMAN_FILTER_H
#define __KALMAN_FILTER_H

#include "stm32f10x.h"
#include "arm_math.h"

// 使用Q1.31定点格式（无FPU优化）
typedef q31_t kfloat;

// 系统维度配置
#define STATE_DIM 4    // [x, y, vx, vy]
#define MEAS_DIM  2    // [x, y]

// 卡尔曼滤波器结构体（全静态内存）
typedef struct {
    arm_matrix_instance_q31 F;   // 状态转移矩阵
    arm_matrix_instance_q31 H;   // 测量矩阵
    arm_matrix_instance_q31 Q;   // 过程噪声协方差
    arm_matrix_instance_q31 R;   // 测量噪声协方差
    arm_matrix_instance_q31 P;   // 误差协方差
    arm_matrix_instance_q31 x;   // 状态向量
} KalmanFilter;

// 初始化滤波器（采样时间0.1s对应10Hz）
void Kalman_Init(KalmanFilter* kf);

// 预测步骤
void Kalman_Predict(KalmanFilter* kf);

// 更新步骤（输入测量值）
void Kalman_Update(KalmanFilter* kf, q31_t* meas);

#endif

/* 文件：kalman_filter.c */
#include "kalman_filter.h"

// 静态内存预分配（Q31定点数）
static q31_t F_data[STATE_DIM*STATE_DIM] = {0};
static q31_t H_data[MEAS_DIM*STATE_DIM] = {0};
static q31_t Q_data[STATE_DIM*STATE_DIM] = {0};
static q31_t R_data[MEAS_DIM*MEAS_DIM] = {0};
static q31_t P_data[STATE_DIM*STATE_DIM] = {0};
static q31_t x_data[STATE_DIM] = {0};

// 临时矩阵缓存（避免动态分配）
static q31_t tmp44[STATE_DIM*STATE_DIM];  // 4x4矩阵缓存
static q31_t tmp42[STATE_DIM*MEAS_DIM];   // 4x2矩阵缓存
static q31_t K_data[STATE_DIM*MEAS_DIM];  // 卡尔曼增益

// Q31格式缩放因子（根据测量范围±10米，精度0.1mm）
#define Q_SCALE  (1LL << 31) / 100000  // 1单位=0.1mm

void Kalman_Init(KalmanFilter* kf) 
{
    /* 状态转移矩阵 F (匀速模型) */
    // F = [[1, dt, 0,  0],
    //      [0,  1, 0,  0],
    //      [0,  0, 1, dt],
    //      [0,  0, 0,  1]]
    arm_fill_q31(0, F_data, STATE_DIM*STATE_DIM);
    F_data[0] = Q_SCALE * 1;   // F[0][0]
    F_data[1] = Q_SCALE * 0.1; // F[0][1] (dt=0.1s)
    F_data[5] = Q_SCALE * 1;   // F[1][1]
    F_data[10] = Q_SCALE * 1;  // F[2][2]
    F_data[11] = Q_SCALE * 0.1;// F[2][3]
    F_data[15] = Q_SCALE * 1;  // F[3][3]
    arm_mat_init_q31(&kf->F, STATE_DIM, STATE_DIM, F_data);

    /* 测量矩阵 H */
    arm_fill_q31(0, H_data, MEAS_DIM*STATE_DIM);
    H_data[0] = Q_SCALE * 1;  // H[0][0]
    H_data[6] = Q_SCALE * 1;  // H[1][2]
    arm_mat_init_q31(&kf->H, MEAS_DIM, STATE_DIM, H_data);

    /* 过程噪声协方差 Q (经验值) */
    const q31_t q_pos = Q_SCALE * 0.01;  // 位置过程噪声
    const q31_t q_vel = Q_SCALE * 0.1;   // 速度过程噪声
    arm_fill_q31(0, Q_data, STATE_DIM*STATE_DIM);
    Q_data[0]  = q_pos;  // x噪声
    Q_data[5]  = q_vel;  // vx噪声
    Q_data[10] = q_pos;  // y噪声
    Q_data[15] = q_vel;  // vy噪声
    arm_mat_init_q31(&kf->Q, STATE_DIM, STATE_DIM, Q_data);

    /* 测量噪声协方差 R (50cm误差) */
    const q31_t r = (Q_SCALE * 0.5) * (Q_SCALE * 0.5); // 方差=0.5^2
    arm_fill_q31(0, R_data, MEAS_DIM*MEAS_DIM);
    R_data[0] = r; 
    R_data[3] = r;
    arm_mat_init_q31(&kf->R, MEAS_DIM, MEAS_DIM, R_data);

    /* 初始协方差矩阵 P */
    arm_fill_q31(0, P_data, STATE_DIM*STATE_DIM);
    P_data[0]  = Q_SCALE * 1;  // x初始方差
    P_data[5]  = Q_SCALE * 1;  // vx初始方差
    P_data[10] = Q_SCALE * 1;  // y初始方差
    P_data[15] = Q_SCALE * 1;  // vy初始方差
    arm_mat_init_q31(&kf->P, STATE_DIM, STATE_DIM, P_data);

    /* 初始状态向量 x */
    arm_fill_q31(0, x_data, STATE_DIM);
    arm_mat_init_q31(&kf->x, STATE_DIM, 1, x_data);
}

void Kalman_Predict(KalmanFilter* kf)
{
    // 状态预测: x = F * x
    arm_mat_mult_q31(&kf->F, &kf->x, &kf->x, tmp44);

    // 协方差预测: P = F * P * F^T + Q
    arm_mat_trans_q31(&kf->F, &kf->F); // F转置
    arm_mat_mult_q31(&kf->F, &kf->P, &kf->P, tmp44);
    arm_mat_mult_q31(&kf->P, &kf->F, &kf->P, tmp44);
    arm_mat_add_q31(&kf->P, &kf->Q, &kf->P);
}

void Kalman_Update(KalmanFilter* kf, q31_t* meas)
{
    arm_matrix_instance_q31 z;
    arm_mat_init_q31(&z, MEAS_DIM, 1, meas);

    // 计算卡尔曼增益 K = P * H^T * inv(H * P * H^T + R)
    arm_matrix_instance_q31 Ht;
    arm_mat_init_q31(&Ht, STATE_DIM, MEAS_DIM, tmp42);
    arm_mat_trans_q31(&kf->H, &Ht);
    
    arm_mat_mult_q31(&kf->H, &kf->P, &kf->H, tmp44);
    arm_mat_mult_q31(&kf->H, &Ht, &kf->H, tmp44);
    arm_mat_add_q31(&kf->H, &kf->R, &kf->H);
    arm_mat_inverse_q31(&kf->H, &kf->H); // 求逆
    
    arm_mat_mult_q31(&kf->P, &Ht, &kf->P, tmp44);
    arm_mat_mult_q31(&kf->P, &kf->H, (arm_matrix_instance_q31*)K_data, tmp44);

    // 更新状态估计
    arm_mat_mult_q31(&kf->H, &kf->x, &kf->x, tmp44);
    arm_mat_sub_q31(&z, &kf->x, &z);
    arm_mat_mult_q31((arm_matrix_instance_q31*)K_data, &z, &kf->x, tmp44);
    arm_mat_add_q31(&kf->x, &kf->x, &kf->x);

    // 更新协方差
    arm_mat_mult_q31((arm_matrix_instance_q31*)K_data, &kf->H, &kf->H, tmp44);
    arm_mat_scale_q31(&kf->H, -1*Q_SCALE, &kf->H);
    arm_mat_add_q31(&kf->P, &kf->H, &kf->P);
}

/* 文件：main.c */
#include "kalman_filter.h"
#include "stm32f10x_usart.h"

KalmanFilter kf;
q31_t meas_buffer[MEAS_DIM];

// UART1接收中断处理
void USART1_IRQHandler(void)
{
    static uint8_t rx_buf[16], idx=0;
    if(USART_GetITStatus(USART1, USART_IT_RXNE)) {
        uint8_t c = USART_ReceiveData(USART1);
        if(c == '\n' && idx >= 8) { // 假设数据格式：x.xxxx,y.yyyy\n
            float x = atof((char*)rx_buf);
            float y = atof((char*)rx_buf + 5);
            meas_buffer[0] = (q31_t)(x * Q_SCALE);
            meas_buffer[1] = (q31_t)(y * Q_SCALE);
            idx = 0;
        } else {
            rx_buf[idx++] = c;
        }
    }
}

int main(void)
{
    SystemInit();
    Kalman_Init(&kf);
    
    // 初始化UART1（波特率115200）
    USART_InitTypeDef USART_InitStruct = {
        .USART_BaudRate = 115200,
        .USART_Mode = USART_Mode_Rx,
        .USART_WordLength = USART_WordLength_8b,
        .USART_StopBits = USART_StopBits_1,
        .USART_Parity = USART_Parity_No
    };
    USART_Init(USART1, &USART_InitStruct);
    USART_Cmd(USART1, ENABLE);
    USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
    NVIC_EnableIRQ(USART1_IRQn);

    while(1) {
        // 10Hz处理循环
        static uint32_t last_tick = 0;
        if(HAL_GetTick() - last_tick >= 100) {
            last_tick = HAL_GetTick();
            
            Kalman_Predict(&kf);
            Kalman_Update(&kf, meas_buffer);
            
            // 获取滤波结果（转换为浮点）
            float filtered_x = (float)kf.x.pData[0] / Q_SCALE;
            float filtered_y = (float)kf.x.pData[2] / Q_SCALE;
            
            // 此处可以输出结果到上位机或其他模块
        }
    }
}

下面是输出的说明:

关键实现说明：
 - 定点数优化：
采用Q31格式（1符号位+31小数位），利用CMSIS-DSP的硬件加速矩阵运算
定义Q_SCALE缩放因子，将实际物理值映射到Q31范围（示例中1单位=0.1mm）
 - 内存控制：
所有矩阵使用静态预分配内存，避免动态分配
临时矩阵复用同一块内存区域（tmp44/tmp42）
 - 实时性保障：
中断接收数据，主循环10Hz定时处理
矩阵运算选择时间复杂度最低的展开方式
 - 噪声参数设定
过程噪声Q根据经验值设定（需实际调试微调）
测量噪声R严格按50cm误差计算方差
 - 调试建议：
通过UART输出原始数据和滤波数据对比
使用Keil的Event Recorder监控运算时间
调整Q_SCALE参数平衡精度和溢出风险
实际路测时用在线参数调整功能优化Q/R矩阵*

该代码已在STM32F103RC硬件平台上验证通过，实测单次滤波耗时约2.1ms（72MHz主频），满足10Hz实时性要求。

上面的代码看起来应该没有问题。关于上面的代码，需要说明以下几点：

上面的要求：“传感器通过UART1/UART2上传x/y坐标”，这一句没有正确理解，只是采用USART1将x/y坐标进行了传递。
关于浮点，要求“浮点：如果无FPU时，采用固定点运算，以加快速度”，这个是做到了，采用了Q1.31定点格式

// 使用Q1.31定点格式（无FPU优化）
typedef q31_t kfloat;

而这个数据类型的本质为：

  /**
   * @brief 32-bit fractional data type in 1.31 format.
   */
  typedef int32_t q31_t;

就是采用定点数来替换浮点数。定点数比浮点数运算时间快，但精度不如浮点数。如果有FPU的情况下优先浮点数运算，没有FPU的情况下，现在这一款MCU是没有FPU的，所以它的处理是对的。

6 结论

虽然有时Deepseek也会胡说八道，答非所问，但是用它来生成代码基本还是可行的，只是要求要明确；它可以帮我们省去不少的时间，当然生成的代码往往还需要修修边，磨磨园；省下的时间我们便可将精力花在Q/R矩阵调节上。但是不是所有的平台DS生成的结果都一样的，不要所托非人，终误君生！