学习卡尔曼滤波（Kalman Filter）并在Stm32上验证

•R：通过实验测量ADC噪声方差。•Q：从小值开始，逐渐调整以平衡平滑度和响应速度。•一维：简单高效，适合静态信号。•二维：能跟踪趋势，适合缓慢变化。•三维：适合复杂动态，需权衡计算成本。希望这篇文章对你的嵌入式学习有帮助！未来可以尝试融合多传感器数据（如IMU+GPS），体验卡尔曼滤波的更多魅力。

青青草原的灰太狼呀

3617人浏览 · 2025-04-15 19:16:18

青青草原的灰太狼呀 · 2025-04-15 19:16:18 发布

卡尔曼滤波（Kalman Filter）是一种强大的信号处理工具，广泛应用于噪声过滤和状态估计。本文将介绍卡尔曼滤波的使用场景和方法，并通过STM32F103的ADC采样实现，分别讲解一维、二维和三维卡尔曼滤波的代码和效果。

一、卡尔曼滤波的使用场景

卡尔曼滤波适用于从带噪声的测量数据中估计系统状态，特别适合以下场景：
   1.   传感器数据平滑：
   •   如ADC采集的电压、温度，或IMU的加速度、角速度，滤除噪声后得到更稳定的信号。
   2.   状态估计：
   •   机器人导航中，融合GPS和IMU数据估计位置和速度。
   •   自动驾驶中，跟踪车辆的运动轨迹。
   3.   预测与控制：
   •   电池管理系统（BMS）中，估计SOC（荷电状态）和SOH（健康状态）。
   •   工业控制中，实时校正传感器偏差。
在嵌入式系统中，卡尔曼滤波因计算效率高、递归性强，非常适合单片机实现。

二、卡尔曼滤波的使用方法

卡尔曼滤波的核心是预测和更新两个步骤，基于线性系统和高斯噪声假设。以下是通用流程：
   1.   系统建模：
   •   定义状态向量（如位置、速度）。
   •   建立状态转移模型（描述状态如何随时间变化）。
   •   定义测量模型（传感器如何观测状态）。
   2.   初始化：
   •   设置初始状态估计、估计协方差、过程噪声协方差（Q）、测量噪声协方差（R）。
   3.   预测：
   •   根据状态转移方程预测下一时刻状态。
   •   更新估计协方差。
   4.   更新：
   •   计算卡尔曼增益（权衡预测和测量）。
   •   结合实际测量值修正状态估计。
   •   更新协方差。
数学公式（以一维为例）：
   •   预测： $\hat{x}_{k|k-1} = \hat{x}_{k-1|k-1}, \quad P_{k|k-1} = P_{k-1|k-1} + Q$
   •   更新：

$K_k = \frac{P_{k|k-1}}{P_{k|k-1} + R}, \quad \hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k (z_k - \hat{x}_{k|k-1}), \quad P_{k|k} = (1 - K_k) P_{k|k-1}$

        其中：
   •   $\hat{x}$ : 状态估计
   •   P: 估计协方差
   •   Q: 过程噪声协方差
   •   R: 测量噪声协方差
   •   K: 卡尔曼增益
   •   $z_k$ : 测量值

再详细点就是
   1.   状态预测：
   •   状态估计： $\hat{x}_{k|k-1} = \hat{x}_{k-1|k-1}$ （假设状态不随时间变化，或有简单的动态模型）。
   •   协方差预测： $P_{k|k-1} = P_{k-1|k-1} + Q$ ，其中 ( Q ) 是过程噪声协方差。
   2.   测量更新：
   •   卡尔曼增益： $K_k = \frac{P_{k|k-1}}{P_{k|k-1} + R}$ ，其中 ( R ) 是测量噪声协方差。
   •   状态更新： $\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k (z_k - \hat{x}_{k|k-1})$ ，其中 ( z_k ) 是ADC测量值。
   •   协方差更新： $P_{k|k} = (1 - K_k) P_{k|k-1}$ 。
关键参数：
   •   $\hat{x}$ ：估计值（滤波后的ADC值）。
   •   P：估计误差协方差，反映估计的不确定性。
   •   Q：过程噪声（系统本身的随机变化，通常较小）。
   •   R：测量噪声（噪声，需根据实际测量估计）。

三、STM32F103 ADC采样实践

我们以STM32F103（Cortex-M3，带12位ADC）为例，通过ADC采集模拟电压信号，分别实现一维、二维和三维卡尔曼滤波。硬件假设：
• 使用PA0（ADC1通道0）采集0-3.3V电压。
• 目标：滤除ADC噪声，得到平滑信号。

3.1 ADC 配置

使用STM32CubeMX配置ADC在这里就不多说了，读取adc转换的值为如下代码。当然也可以直接对adc_value进行滤波。代码基于HAL库：


float Read_ADC(void) {
    HAL_ADC_Start(&hadc1);
    HAL_ADC_PollForConversion(&hadc1, HAL_MAX_DELAY);
    uint16_t adc_value = HAL_ADC_GetValue(&hadc1);
    return (float)adc_value * 3.3f / 4095.0f; // 转换为电压
}

3.2 一维卡尔曼滤波

直接平滑ADC采集的电压值，假设电压变化缓慢（无明显动态）。
模型
   •   状态：电压值 $x$ 。
   •   状态转移： $x_k = x_{k-1}$ （静态系统）。
   •   测量： $z_k = x_k + v_k$ ，其中 $v_k$ 是ADC噪声。

代码如下：

typedef struct {
    float x; // 状态估计（电压）
    float P; // 估计协方差
    float Q; // 过程噪声协方差
    float R; // 测量噪声协方差
} Kalman1D;

void Kalman1D_Init(Kalman1D *kf, float initial_x, float initial_P, float Q, float R) {
    kf->x = initial_x;
    kf->P = initial_P;
    kf->Q = Q;
    kf->R = R;
}

float Kalman1D_Update(Kalman1D *kf, float measurement) {
    // 预测
    kf->P = kf->P + kf->Q;

    // 更新
    float K = kf->P / (kf->P + kf->R);
    kf->x = kf->x + K * (measurement - kf->x);
    kf->P = (1 - K) * kf->P;

    return kf->x;
}

Kalman1D kf;

int main(void) {
    HAL_Init();
    /*
中间初始化代码
*/
    Kalman1D_Init(&kf, 0.0f, 1.0f, 0.001f, 0.1f); // Q=0.001, R=0.1

    while (1) {
        float voltage = Read_ADC();
        float filtered_voltage = Kalman1D_Update(&kf, voltage);
        printf("Raw: %.3f V, Filtered: %.3f V\n", voltage, filtered_voltage);
        HAL_Delay(10);
    }
}

参数说明
   •   R：通过采集静止信号，计算ADC电压方差（比如0.1）。
   •   Q：设为小值（如0.001），因为电压变化慢。
   •   效果：假设输入1.5V带噪声信号（1.4-1.6V抖动），滤波后稳定在1.5V左右。

3.3 二维卡尔曼滤波

假设电压有缓慢变化趋势（如传感器漂移），我们跟踪电压和其变化率（类似速度）。
模型
   •   状态： $\mathbf{x} = [x, \dot{x}]^T$ ，其中 $x$ 是电压， $\dot{x}$ 是电压变化率。
   •   状态转移： $\mathbf{x}k = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \mathbf{x}_{k-1} +\mathbf{w}_k$ 其中 $\Delta t$ 是采样间隔（如0.01s）。
   •   测量： $z_k = [1, 0] \mathbf{x}_k + v_k$ （仅测量电压）。

代码实现:

#include <arm_math.h> // 使用ARM CMSIS库进行矩阵运算

typedef struct {
    float x[2]; // 状态 [电压, 变化率]
    float P[2][2]; // 协方差矩阵
    float Q[2][2]; // 过程噪声协方差
    float R; // 测量噪声协方差
    float dt; // 采样间隔
} Kalman2D;

void Kalman2D_Init(Kalman2D *kf, float initial_x, float initial_dx, float dt) {
    kf->x[0] = initial_x;
    kf->x[1] = initial_dx;
    kf->P[0][0] = 1.0f; kf->P[0][1] = 0.0f;
    kf->P[1][0] = 0.0f; kf->P[1][1] = 1.0f;
    kf->Q[0][0] = 0.001f; kf->Q[0][1] = 0.0f;
    kf->Q[1][0] = 0.0f; kf->Q[1][1] = 0.001f;
    kf->R = 0.1f;
    kf->dt = dt;
}

float Kalman2D_Update(Kalman2D *kf, float measurement) {
    // 状态转移矩阵
    float A[2][2] = {{1, kf->dt}, {0, 1}};
    float H[2] = {1, 0}; // 测量矩阵

    // 预测
    float x_pred[2];
    x_pred[0] = A[0][0] * kf->x[0] + A[0][1] * kf->x[1];
    x_pred[1] = A[1][0] * kf->x[0] + A[1][1] * kf->x[1];

    float P_pred[2][2];
    P_pred[0][0] = A[0][0] * (kf->P[0][0] * A[0][0] + kf->P[0][1] * A[1][0]) +
                   A[0][1] * (kf->P[1][0] * A[0][0] + kf->P[1][1] * A[1][0]) + kf->Q[0][0];
    // 类似计算其他元素...

    // 更新
    float innovation = measurement - (H[0] * x_pred[0] + H[1] * x_pred[1]);
    float S = H[0] * (P_pred[0][0] * H[0] + P_pred[0][1] * H[1]) +
              H[1] * (P_pred[1][0] * H[0] + P_pred[1][1] * H[1]) + kf->R;
    float K[2];
    K[0] = (P_pred[0][0] * H[0] + P_pred[0][1] * H[1]) / S;
    K[1] = (P_pred[1][0] * H[0] + P_pred[1][1] * H[1]) / S;

    kf->x[0] = x_pred[0] + K[0] * innovation;
    kf->x[1] = x_pred[1] + K[1] * innovation;

    // 更新协方差（简化）
    kf->P[0][0] = (1 - K[0] * H[0]) * P_pred[0][0];
    // 类似更新其他元素...

    return kf->x[0];
}


/**************
    主程序
*************/

Kalman2D kf;

int main(void) {
    HAL_Init();
    ADC_Init();
    Kalman2D_Init(&kf, 0.0f, 0.0f, 0.01f);

    while (1) {
        float voltage = Read_ADC();
        float filtered_voltage = Kalman2D_Update(&kf, voltage);
        printf("Raw: %.3f V, Filtered: %.3f V\n", voltage, filtered_voltage);
        HAL_Delay(10);
    }
}

参数与效果
• R, Q：类似一维，Q可稍大（如0.01）以适应变化率。
• 效果：能跟踪缓慢变化的电压（如线性漂移），比一维更平滑

3.4 三维卡尔曼滤波

假设电压有更复杂的动态（如加速度模型），跟踪电压、变化率和加速度。
模型
• 状态： $\mathbf{x} = [x, \dot{x}, \ddot{x}]^T$ 。表示电压，变化率，加速度。
• 状态转移： $\mathbf{x}k = \begin{bmatrix} 1 & \Delta t & \frac{\Delta t^2}{2} \\ 0 & 1 & \Delta t \\ 0 & 0 & 1 \end{bmatrix} \mathbf{x}_{k-1} + \mathbf{w}_k$

$\mathbf{w}_k$ 是过程噪声，协方差矩阵为 $Q$ 。
• 测量： $z_k = [1, 0, 0] \mathbf{x}_k + v_k$ 。

因为ADC只是测量电压， $v_k$ 是测量噪声，协方差为 $R$

噪声参数说明：

$Q$ ：过程噪声协方差，设为对角矩阵，反映状态变化的不确定性。

$R$ ：测量噪声协方差，通过实验估计ADC噪声

代码实现：

共有两种一种不使用CMSIS-DSP库，简单的矩阵运算另一种使用CMSIS-DSP库

不使用CMSIS-DSP库：

#include "stm32f1xx_hal.h"
#include <stdio.h>


// 三维卡尔曼滤波器结构体
typedef struct {
    float x[3];        // 状态向量 [电压, 变化率, 加速度]
    float P[3][3];     // 估计协方差矩阵
    float Q[3][3];     // 过程噪声协方差矩阵
    float R;           // 测量噪声协方差
    float dt;          // 采样间隔（秒）
} Kalman3D;


// 初始化三维卡尔曼滤波器
void Kalman3D_Init(Kalman3D *kf, float initial_x, float initial_dx, float initial_ddx, float dt) {
    // 初始状态
    kf->x[0] = initial_x;   // 初始电压
    kf->x[1] = initial_dx;  // 初始变化率
    kf->x[2] = initial_ddx; // 初始加速度

    // 初始协方差矩阵（单位矩阵，表示初始不确定性）
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            kf->P[i][j] = (i == j) ? 1.0f : 0.0f;
        }
    }

    // 过程噪声协方差（对角矩阵，假设各状态独立）
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            kf->Q[i][j] = (i == j) ? 0.001f : 0.0f;
        }
    }

    // 测量噪声协方差（通过实验估计）
    kf->R = 0.1f;

    // 采样间隔
    kf->dt = dt;
}

// 三维卡尔曼滤波更新
float Kalman3D_Update(Kalman3D *kf, float measurement) {
    // 状态转移矩阵 A
    float A[3][3] = {
        {1.0f, kf->dt, 0.5f * kf->dt * kf->dt},
        {0.0f, 1.0f, kf->dt},
        {0.0f, 0.0f, 1.0f}
    };

    // 测量矩阵 H
    float H[3] = {1.0f, 0.0f, 0.0f};

    // 临时变量
    float x_pred[3];    // 预测状态
    float P_pred[3][3]; // 预测协方差
    float K[3];         // 卡尔曼增益

    // 1. 预测步骤
    // 状态预测：x_pred = A * x
    for (int i = 0; i < 3; i++) {
        x_pred[i] = 0.0f;
        for (int j = 0; j < 3; j++) {
            x_pred[i] += A[i][j] * kf->x[j];
        }
    }

    // 协方差预测：P_pred = A * P * A^T + Q
    float temp[3][3] = {0};
    // temp = A * P
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            for (int k = 0; k < 3; k++) {
                temp[i][j] += A[i][k] * kf->P[k][j];
            }
        }
    }
    // P_pred = temp * A^T + Q
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            P_pred[i][j] = 0.0f;
            for (int k = 0; k < 3; k++) {
                P_pred[i][j] += temp[i][k] * A[j][k]; // A^T 是 A 的转置
            }
            P_pred[i][j] += kf->Q[i][j];
        }
    }

    // 2. 更新步骤
    // 创新（测量残差）：y = z - H * x_pred
    float y = measurement - (H[0] * x_pred[0] + H[1] * x_pred[1] + H[2] * x_pred[2]);

    // 创新协方差：S = H * P_pred * H^T + R
    float S = 0.0f;
    float temp_H[3] = {0}; // temp_H = P_pred * H^T
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            temp_H[i] += P_pred[i][j] * H[j];
        }
    }
    S = H[0] * temp_H[0] + H[1] * temp_H[1] + H[2] * temp_H[2] + kf->R;

    // 卡尔曼增益：K = P_pred * H^T / S
    for (int i = 0; i < 3; i++) {
        K[i] = temp_H[i] / S;
    }

    // 状态更新：x = x_pred + K * y
    for (int i = 0; i < 3; i++) {
        kf->x[i] = x_pred[i] + K[i] * y;
    }

    // 协方差更新：P = (I - K * H) * P_pred
    float I_KH[3][3] = {0};
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            I_KH[i][j] = (i == j ? 1.0f : 0.0f) - K[i] * H[j];
        }
    }
    // P = I_KH * P_pred
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            kf->P[i][j] = 0.0f;
            for (int k = 0; k < 3; k++) {
                kf->P[i][j] += I_KH[i][k] * P_pred[k][j];
            }
        }
    }

    return kf->x[0]; // 返回滤波后的电压
}

// 主程序
Kalman3D kf;

int main(void) {
    HAL_Init();
    ADC_Init();

    // 初始化三维卡尔曼滤波器
    Kalman3D_Init(&kf, 0.0f, 0.0f, 0.0f, 0.01f); // 初始电压=0, dt=0.01s

    while (1) {
        // 读取 ADC
        float voltage = Read_ADC();

        // 卡尔曼滤波
        float filtered_voltage = Kalman3D_Update(&kf, voltage);

        // 输出结果（通过串口）
        printf("Raw: %.3f V, Filtered: %.3f V\n", voltage, filtered_voltage);

        HAL_Delay(10); // 每10ms采样一次
    }
}

带CMSIS-DSP

使用arm的CMSIS_DSP库，适用于高频或多维使用

#include "stm32f1xx_hal.h"
#include "arm_math.h"
#include <stdio.h>

// ADC 句柄（需在 STM32CubeMX 中配置）
extern ADC_HandleTypeDef hadc1;

// 三维卡尔曼滤波器结构体
typedef struct {
    arm_matrix_instance_f32 x; // 状态向量 [电压, 变化率, 加速度] (3x1)
    arm_matrix_instance_f32 P; // 估计协方差矩阵 (3x3)
    arm_matrix_instance_f32 Q; // 过程噪声协方差矩阵 (3x3)
    float R;                  // 测量噪声协方差
    float dt;                 // 采样间隔（秒）
    // 辅助矩阵（预分配，避免动态分配）
    arm_matrix_instance_f32 A;    // 状态转移矩阵 (3x3)
    arm_matrix_instance_f32 H;    // 测量矩阵 (1x3)
    arm_matrix_instance_f32 x_pred; // 预测状态 (3x1)
    arm_matrix_instance_f32 P_pred; // 预测协方差 (3x3)
    arm_matrix_instance_f32 K;     // 卡尔曼增益 (3x1)
} Kalman3D;

// 矩阵数据存储
float x_data[3];        // 状态向量
float P_data[9];        // 协方差矩阵 (3x3)
float Q_data[9];        // 过程噪声协方差
float A_data[9];        // 状态转移矩阵
float H_data[3];        // 测量矩阵
float x_pred_data[3];   // 预测状态
float P_pred_data[9];   // 预测协方差
float K_data[3];        // 卡尔曼增益


// 初始化三维卡尔曼滤波器
void Kalman3D_Init(Kalman3D *kf, float initial_x, float initial_dx, float initial_ddx, float dt) {
    // 初始化状态向量 x
    x_data[0] = initial_x;
    x_data[1] = initial_dx;
    x_data[2] = initial_ddx;
    arm_mat_init_f32(&kf->x, 3, 1, x_data);

    // 初始化协方差矩阵 P (单位矩阵)
    for (int i = 0; i < 9; i++) {
        P_data[i] = (i % 4 == 0) ? 1.0f : 0.0f; // 对角线为1
    }
    arm_mat_init_f32(&kf->P, 3, 3, P_data);

    // 初始化过程噪声协方差 Q (对角矩阵)
    for (int i = 0; i < 9; i++) {
        Q_data[i] = (i % 4 == 0) ? 0.001f : 0.0f; // 对角线为0.001
    }
    arm_mat_init_f32(&kf->Q, 3, 3, Q_data);

    // 初始化状态转移矩阵 A
    A_data[0] = 1.0f; A_data[1] = dt; A_data[2] = 0.5f * dt * dt;
    A_data[3] = 0.0f; A_data[4] = 1.0f; A_data[5] = dt;
    A_data[6] = 0.0f; A_data[7] = 0.0f; A_data[8] = 1.0f;
    arm_mat_init_f32(&kf->A, 3, 3, A_data);

    // 初始化测量矩阵 H
    H_data[0] = 1.0f; H_data[1] = 0.0f; H_data[2] = 0.0f;
    arm_mat_init_f32(&kf->H, 1, 3, H_data);

    // 初始化预测状态和协方差
    arm_mat_init_f32(&kf->x_pred, 3, 1, x_pred_data);
    arm_mat_init_f32(&kf->P_pred, 3, 3, P_pred_data);

    // 初始化卡尔曼增益
    arm_mat_init_f32(&kf->K, 3, 1, K_data);

    // 测量噪声协方差
    kf->R = 0.1f;

    // 采样间隔
    kf->dt = dt;
}

// 三维卡尔曼滤波更新
float Kalman3D_Update(Kalman3D *kf, float measurement) {
    // 临时矩阵
    float temp1_data[9]; // 3x3
    float temp2_data[3]; // 3x1
    float temp3_data[1]; // 1x1
    arm_matrix_instance_f32 temp1, temp2, temp3;
    arm_mat_init_f32(&temp1, 3, 3, temp1_data);
    arm_mat_init_f32(&temp2, 3, 1, temp2_data);
    arm_mat_init_f32(&temp3, 1, 1, temp3_data);

    // 1. 预测步骤
    // 状态预测：x_pred = A * x
    arm_mat_mult_f32(&kf->A, &kf->x, &kf->x_pred);

    // 协方差预测：P_pred = A * P * A^T + Q
    arm_mat_mult_f32(&kf->A, &kf->P, &temp1); // temp1 = A * P
    arm_mat_trans_f32(&kf->A, &temp1);        // temp1 = A^T (复用 temp1)
    arm_mat_mult_f32(&temp1, &kf->A, &kf->P_pred); // P_pred = (A * P) * A^T
    arm_mat_add_f32(&kf->P_pred, &kf->Q, &kf->P_pred); // P_pred += Q

    // 2. 更新步骤
    // 创新：y = z - H * x_pred
    arm_mat_mult_f32(&kf->H, &kf->x_pred, &temp3); // temp3 = H * x_pred
    float y = measurement - temp3_data[0];

    // 创新协方差：S = H * P_pred * H^T + R
    arm_mat_mult_f32(&kf->H, &kf->P_pred, &temp2); // temp2 = H * P_pred
    arm_mat_trans_f32(&kf->H, &temp1);             // temp1 = H^T
    arm_mat_mult_f32(&temp2, &temp1, &temp3);      // temp3 = (H * P_pred) * H^T
    float S = temp3_data[0] + kf->R;

    // 卡尔曼增益：K = P_pred * H^T / S
    arm_mat_trans_f32(&kf->H, &temp1); // temp1 = H^T
    arm_mat_mult_f32(&kf->P_pred, &temp1, &kf->K); // K = P_pred * H^T
    arm_mat_scale_f32(&kf->K, 1.0f / S, &kf->K);   // K /= S

    // 状态更新：x = x_pred + K * y
    arm_mat_scale_f32(&kf->K, y, &temp2); // temp2 = K * y
    arm_mat_add_f32(&kf->x_pred, &temp2, &kf->x); // x = x_pred + K * y

    // 协方差更新：P = (I - K * H) * P_pred
    arm_mat_mult_f32(&kf->K, &kf->H, &temp1); // temp1 = K * H
    arm_mat_scale_f32(&temp1, -1.0f, &temp1); // temp1 = -K * H
    for (int i = 0; i < 9; i += 4) {
        temp1_data[i] += 1.0f; // temp1 += I (单位矩阵)
    }
    arm_mat_mult_f32(&temp1, &kf->P_pred, &kf->P); // P = (I - K * H) * P_pred

    return kf->x.pData[0]; // 返回滤波后的电压
}

// 主程序
Kalman3D kf;

int main(void) {
    HAL_Init();
    ADC_Init();

    // 初始化三维卡尔曼滤波器
    Kalman3D_Init(&kf, 0.0f, 0.0f, 0.0f, 0.01f); // 初始电压=0, dt=0.01s

    while (1) {
        // 读取 ADC
        float voltage = Read_ADC();

        // 卡尔曼滤波
        float filtered_voltage = Kalman3D_Update(&kf, voltage);

        // 输出结果（通过串口）
        printf("Raw: %.3f V, Filtered: %.3f V\n", voltage, filtered_voltage);

        HAL_Delay(10); // 每10ms采样一次
    }
}

python模拟效果，参数一样时：

四、总结与调试

   参数调优：
   •   R：通过实验测量ADC噪声方差。
   •   Q：从小值开始，逐渐调整以平衡平滑度和响应速度。
通过一维到三维的实现，我们看到卡尔曼滤波的灵活性：
   •   一维：简单高效，适合静态信号。
   •   二维：能跟踪趋势，适合缓慢变化。
   •   三维：适合复杂动态，需权衡计算成本。
希望这篇文章对你的嵌入式学习有帮助！未来可以尝试融合多传感器数据（如IMU+GPS），体验卡尔曼滤波的更多魅力。