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

今天开组会，中午没睡困得要死，但是每天都得不停学习。

参考下面视频

卡尔曼滤波

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

---

卡尔曼滤波的核心思想

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

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

---

卡尔曼滤波的数学模型

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

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

$$x_k=F_k{x}_{k-1}+B_k{u}_k+w_k$$

• $x_k$：系统在时刻 $k$ 的状态向量（例如位置、速度）。
• $F_k$：状态转移矩阵，描述状态之间的变化关系。
• $B_k$：控制输入矩阵，描述控制输入对状态的影响。
• $u_k$：控制输入。
• $w_k$：过程噪声，服从均值为 $0$，协方差为 $Q$ 的高斯分布。
  

2. 观测模型（预测测量值）

$$z_k=H_k{x}_k+v_k$$

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

---

卡尔曼滤波的五个关键步骤

1. 预测状态

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

2. 预测误差协方差

预测状态的误差协方差：
$$P_k^{-}=F_k{P}_{k-1}{F}_k^T+Q$$

• $P_k^{-}$：预测误差协方差矩阵，反映预测值的不确定性。
• $Q$：过程噪声协方差矩阵。

3. 计算卡尔曼增益

根据预测的不确定性和测量的不确定性计算卡尔曼增益：
$$K_k=P_k^{-}{H}_k^T(H_k{P}_k^{-}{H}_k^T+R{)}^{-1}$$

• $K_k$：卡尔曼增益，决定了预测和测量数据的权重。

4. 更新状态

结合测量值更新状态估计：
$${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H_k{\hat{x}}_k^{-})$$

• $z_k-H_k{\hat{x}}_k^{-}$：测量残差（即预测值与观测值的差异）。

5. 更新误差协方差

更新误差协方差矩阵：
$$P_k=(I-K_k{H}_k)P_k^{-}$$

• $I$：单位矩阵。

---

卡尔曼滤波算法步骤总结

1. 初始化：设定初始状态 ${\hat{x}}_0$ 和初始协方差矩阵 $P_0$。
2. 循环执行：
   • 预测状态 ${\hat{x}}_k^{-}$ 和协方差 $P_k^{-}$。
   • 计算卡尔曼增益 $K_k$。
   • 更新状态 ${\hat{x}}_k$ 和协方差 $P_k$。

---

代码实现（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 @ uP_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)  # 连接到指定 WiFiwhile not wlan.isconnected():  # 等待连接passprint("Connected to WiFi!")print("IP Address:", wlan.ifconfig()[0])  # 打印 IP 地址# 连接到 WiFi
connect_wifi("your_ssid", "your_password")  # 替换为你的 WiFi 名称和密码# 主循环：控制 LED 闪烁
while True:led.value(1)  # 点亮 LEDtime.sleep(1)  # 等待 1 秒led.value(0)  # 熄灭 LEDtime.sleep(1)  # 等待 1 秒

代码说明

1. GPIO 控制：

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

2. WiFi 连接：

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

开发环境搭建

1. 固件烧录：

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

2. IDE 选择：

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