嵌入式学习笔记-卡尔曼滤波,PID,MicroPython

文章目录 卡尔曼滤波卡尔曼滤波的核心思想卡尔曼滤波的数学模型1. 状态转移模型（预测系统状态）2. 观测模型（预测测量值） 卡尔曼滤波的五个关键步骤1. 预测状态2. 预测误差协方差3. 计算卡尔曼增益4. 更新状态5. 更新误差协方差 卡尔曼滤波算法步骤总结代码实现（Python 示例） PID调节总结 MicroPython示例代码：控制 LED 灯并连接 WiFi1. 硬件准备2. 连接方式3. 示例代码 代码说明开发环境搭建 今天开组会，中午没睡困得要死，但是每天都得不停学习。

参考下面视频

卡尔曼滤波

卡尔曼滤波是一种用于估计动态系统状态的算法，它是一种线性递归滤波 器，即使测量数据中存在噪声，它也能提供对真实状态的最佳估计。这种滤波算法在控制系统、导航、信号处理等领域应用广泛。

---

卡尔曼滤波的核心思想

卡尔曼滤波是一种递归估计算法，它在每个时刻执行两步：

预测阶段：根据上一时刻的状态，利用系统的动态模型预测当前时刻的状态。更新阶段：结合当前的测量数据，校正预测值，得到更准确的状态估计。

---

卡尔曼滤波的数学模型

卡尔曼滤波假设系统可以用线性模型描述，并且噪声服从高斯分布。系统模型由以下两部分组成：

1. 状态转移模型（预测系统状态）

x k = F k x k − 1 + B k u k + w k x_k = F_k x_{k-1} + B_k u_k + w_k xk​=Fk​xk−1​+Bk​uk​+wk​

x k x_k xk​：系统在时刻 k k k 的状态向量（例如位置、速度）。 F k F_k Fk​：状态转移矩阵，描述状态之间的变化关系。 B k B_k Bk​：控制输入矩阵，描述控制输入对状态的影响。 u k u_k uk​：控制输入。 w k w_k wk​：过程噪声，服从均值为 0 0 0，协方差为 Q Q Q 的高斯分布。 2. 观测模型（预测测量值）

z k = H k x k + v k z_k = H_k x_k + v_k zk​=Hk​xk​+vk​

z k z_k zk​：时刻 k k k 的观测值。 H k H_k Hk​：观测矩阵，描述状态如何映射到观测空间。 v k v_k vk​：测量噪声，服从均值为 0 0 0，协方差为 R R R 的高斯分布。

---

卡尔曼滤波的五个关键步骤 1. 预测状态

根据状态转移模型，预测当前时刻的状态： x ^ k − = F k x ^ k − 1 + B k u k \hat{x}_k^- = F_k \hat{x}_{k-1} + B_k u_k x^k−​=Fk​x^k−1​+Bk​uk​ 这里， x ^ k − \hat{x}_k^- x^k−​ 表示预测的状态。

2. 预测误差协方差

预测状态的误差协方差： P k − = F k P k − 1 F k T + Q P_k^- = F_k P_{k-1} F_k^T + Q Pk−​=Fk​Pk−1​FkT​+Q

P k − P_k^- Pk−​：预测误差协方差矩阵，反映预测值的不确定性。 Q Q Q：过程噪声协方差矩阵。 3. 计算卡尔曼增益

根据预测的不确定性和测量的不确定性计算卡尔曼增益： K k = P k − H k T ( H k P k − H k T + R ) − 1 K_k = P_k^- H_k^T (H_k P_k^- H_k^T + R)^{-1} Kk​=Pk−​HkT​(Hk​Pk−​HkT​+R)−1

K k K_k Kk​：卡尔曼增益，决定了预测和测量数据的权重。 4. 更新状态

结合测量值更新状态估计： x ^ k = x ^ k − + K k ( z k − H k x ^ k − ) \hat{x}_k = \hat{x}_k^- + K_k (z_k - H_k \hat{x}_k^-) x^k​=x^k−​+Kk​(zk​−Hk​x^k−​)

z k − H k x ^ k − z_k - H_k \hat{x}_k^- zk​−Hk​x^k−​：测量残差（即预测值与观测值的差异）。 5. 更新误差协方差

更新误差协方差矩阵： P k = ( I − K k H k ) P k − P_k = (I - K_k H_k) P_k^- Pk​=(I−Kk​Hk​)Pk−​

I I I：单位矩阵。

---

卡尔曼滤波算法步骤总结 初始化：设定初始状态 x ^ 0 \hat{x}_0 x^0​ 和初始协方差矩阵 P 0 P_0 P0​。循环执行： 预测状态 x ^ k − \hat{x}_k^- x^k−​ 和协方差 P k − P_k^- Pk−​。计算卡尔曼增益 K k K_k Kk​。更新状态 x ^ k \hat{x}_k x^k​ 和协方差 P k P_k Pk​。

---

代码实现（Python 示例）

以位置和速度为状态（经典一维运动模型）为例：

import numpy as np # 初始化参数 F = np.array([[1, 1], [0, 1]]) # 状态转移矩阵 H = np.array([[1, 0]]) # 观测矩阵 Q = np.array([[1, 0], [0, 1]]) # 过程噪声协方差 R = np.array([[10]]) # 测量噪声协方差 B = np.array([[0], [0]]) # 控制输入矩阵 u = np.array([[0]]) # 控制输入 # 初始状态和协方差 x = np.array([[0], [1]]) # 初始状态：[位置, 速度] P = np.array([[1, 0], [0, 1]]) # 初始误差协方差 # 模拟观测数据 z_real = [5, 6, 7, 9, 10] # 实际观测值 for z in z_real: # 预测阶段 x_predict = F @ x + B @ u P_predict = F @ P @ F.T + Q # 计算卡尔曼增益 K = P_predict @ H.T @ np.linalg.inv(H @ P_predict @ H.T + R) # 更新阶段 z = np.array([[z]]) # 当前观测值 x = x_predict + K @ (z - H @ x_predict) P = (np.eye(2) - K @ H) @ P_predict # 输出当前状态估计 print("位置估计:", x[0, 0], "速度估计:", x[1, 0])

---

PID调节

之前学过点，所以就简单学下怎么调节。

环节功能描述作用优点缺点实际应用注意事项P（比例环节）将比例系数与偏差信号e(t)相乘后输出快速响应系统误差，调节系统向误差减小的方向移动响应迅速，能快速调整系统可能引起系统震荡，存在稳态误差1. 比例增益系数KP不能过大，以避免震荡。 通过逐步增大KP，找到系统等幅振荡时的KP值，取其70%作为最优比例增益系数。I（积分环节）对偏差信号e(t)进行积分，使输出持续增加或减少，直至偏差为零消除稳态误差消除稳态误差，提高系统精度可能引起系统惯性干扰，导致响应变慢1. 在无过谐振荡、稳态误差近似为0时，积分时间常数TI取值合理。 与比例环节结合（PI控制），可同时提高响应速度和消除稳态误差。D（微分环节）对偏差信号e(t)的变化率进行反馈，提前修正偏差减少调节时间，提前校正系统提高动态性能，减少超调对噪声敏感，可能导致误调节1. 在动态系统中，微分环节可以提前发出校正信号，防止偏差扩大。 总结 P环节：快速响应，但可能导致震荡和稳态误差。I环节：消除稳态误差，但可能使系统响应变慢。D环节：提前校正偏差，提高动态性能，但对噪声敏感。

三者结合（PID控制）可综合各环节的优点，克服单一环节的缺点，广泛应用于闭环控制系统中，以实现快速、稳定且无误差的控制效果。感觉PID可以对应快准稳。P越大，冲的越快，I适度，可以用来调节冲刺到最后的精度，D适度可以用来调价波痕，更加稳定。

MicroPython

MicroPython 是一种轻量级的 Python 解释器，专为嵌入式系统和微控制器设计，能够在资源受限的硬件上运行。它保留了 Python 的核心语法，同时提供了丰富的硬件控制接口，非常适合在 ESP32 等单片机上开发。

以下是一个基于 ESP32 单片机的 MicroPython 示例代码，介绍如何通过 GPIO 控制 LED 灯的闪烁，并连接 WiFi 网络。

示例代码：控制 LED 灯并连接 WiFi 1. 硬件准备 ESP32 开发板一个 LED 灯电阻（可选）杜邦线若干 2. 连接方式

将 LED 的正极连接到 ESP32 的 GPIO 引脚（如 GPIO 5），负极通过电阻连接到 GND。

3. 示例代码

以下代码展示了如何控制 GPIO 引脚来点亮和熄灭 LED，并连接到 WiFi 网络。

# 导入必要的模块 from machine import Pin import network import time # 初始化 LED 引脚 led = Pin(5, Pin.OUT) # GPIO 5 作为输出 # 定义 WiFi 连接函数 def connect_wifi(ssid, password): wlan = network.WLAN(network.STA_IF) # 创建 WLAN 对象 wlan.active(True) # 激活 WiFi 接口 if not wlan.isconnected(): # 检查是否已连接 print("Connecting to WiFi...") wlan.connect(ssid, password) # 连接到指定 WiFi while not wlan.isconnected(): # 等待连接 pass print("Connected to WiFi!") print("IP Address:", wlan.ifconfig()[0]) # 打印 IP 地址 # 连接到 WiFi connect_wifi("your_ssid", "your_password") # 替换为你的 WiFi 名称和密码 # 主循环：控制 LED 闪烁 while True: led.value(1) # 点亮 LED time.sleep(1) # 等待 1 秒 led.value(0) # 熄灭 LED time.sleep(1) # 等待 1 秒 代码说明

GPIO 控制：

使用 machine.Pin 模块控制 GPIO 引脚。Pin(5, Pin.OUT) 表示将 GPIO 5 设置为输出模式。led.value(1) 和 led.value(0) 分别用于点亮和熄灭 LED。

WiFi 连接：

使用 network 模块连接到 WiFi 网络。network.WLAN(network.STA_IF) 创建一个 WiFi 客户端对象。wlan.connect(ssid, password) 用于连接到指定的 WiFi 网络。 开发环境搭建

固件烧录：

从 MicroPython 官网下载适用于 ESP32 的固件。使用 esptool.py 工具将固件烧录到 ESP32 开发板。

IDE 选择：

推荐使用 Thonny IDE 或 uPyCraft IDE。在 Thonny 中配置 MicroPython 解释器，并连接到 ESP32 的串口。