夜话卡尔曼滤波(3) - 请Deepseek帮帮忙

原创

已于 2025-03-11 20:20:50 修改 · 449 阅读

4 ·

CC 4.0 BY-SA版权

文章标签：

#stm32 #deepseek #卡尔曼滤波

于 2025-03-11 16:20:10 首次发布

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