2. 基础数学回顾

各位同学，咱们今天聊点硬核的。别怕，我尽量说得接地气。

做决策算法和速度约束融合，说白了就是让机器在有限时间内做对选择。这背后离不开数学。我个人习惯把数学工具分成三块：向量与矩阵（描述状态空间）、概率论（量化不确定性）、贝叶斯定理（动态更新信念）。这三块搞明白了，后面的课程你会轻松很多。

1. 向量：状态空间的基本单元

向量是什么？说白了就是一组有序的数字。比如一辆车的位置 (x, y, θ)，这就是一个三维向量。我在做自动驾驶项目时，经常用向量表示车辆的状态——位置、速度、朝向角。

举个例子：

# 用向量表示车辆状态
import numpy as np

# 位置向量 (x, y)
position = np.array([3.5, 2.1])

# 速度向量 (vx, vy)
velocity = np.array([1.0, 0.5])

# 状态向量 = 位置 + 速度
state = np.concatenate([position, velocity])
print(state)  # [3.5, 2.1, 1.0, 0.5]

嗯，这里要注意：向量的维度决定了状态空间的复杂度。你想想看，如果一辆车有10个状态变量，那状态空间就是10维的。我曾经在项目中遇到过维度爆炸的问题——状态向量从6维加到12维，计算量直接翻了4倍。

2. 矩阵：状态变换的利器

矩阵，说白了就是向量的集合。但它的威力在于——可以描述状态之间的变换关系。

比如状态转移矩阵 A，它告诉你：当前状态 s_t 经过一步操作后，会变成 s_{t+1}。我建议你把矩阵想象成一个「转换器」，输入一个向量，输出另一个向量。

# 状态转移矩阵示例
A = np.array([[1.0, 0.1],  # 位置更新
              [0.0, 1.0]]) # 速度保持不变

# 当前状态 [位置, 速度]
s_t = np.array([10.0, 2.0])

# 预测下一时刻状态
s_t1 = A @ s_t
print(s_t1)  # [10.2, 2.0]

为什么会这样？因为矩阵乘法本质上就是线性变换。在速度约束融合中，我们经常用矩阵表示约束条件——比如加速度限制、转向角限制，都可以写成矩阵不等式。

矩阵类型	在决策算法中的作用	我踩过的坑
状态转移矩阵 A	预测下一时刻状态	忘记考虑时间步长，导致预测偏差
观测矩阵 H	从状态映射到观测值	维度不匹配，矩阵乘法报错
协方差矩阵 Σ	描述状态的不确定性	非正定矩阵导致卡尔曼滤波发散

3. 概率论基础：不确定性是常态

做嵌入式系统的人都知道，传感器数据永远有噪声。概率论就是用来处理这些不确定性的。

三个核心概念：

• 概率分布：描述随机变量取值的可能性。比如GPS测量误差服从高斯分布 N(0, σ²)。
• 条件概率：P(A|B) 表示在B发生的情况下A发生的概率。这在决策中太常用了——给定当前观测，状态的概率是多少？
• 期望与方差：期望是平均值，方差是离散程度。我建议你重点关注方差——它直接反映了不确定性的大小。

# 高斯分布示例
import numpy as np

# 传感器测量值（带噪声）
measurements = np.random.normal(loc=5.0, scale=0.5, size=100)

# 计算均值和方差
mean = np.mean(measurements)      # 约 5.0
variance = np.var(measurements)   # 约 0.25

print(f"均值: {mean:.3f}, 方差: {variance:.3f}")

你想想看，如果方差很大，说明传感器不可靠，决策时就要更保守。我在项目中遇到过激光雷达在雨天的方差暴增3倍，如果不做概率建模，直接拿原始数据做决策，后果就是车辆乱打方向盘。

4. 贝叶斯定理：信念更新的核心

贝叶斯定理，说白了就是「根据新证据更新旧信念」。公式很简单：

P(状态 | 观测) ∝ P(观测 | 状态) × P(状态)

翻译成人话：

• P(状态) 是「先验」——你之前对状态的猜测
• P(观测 | 状态) 是「似然」——给定状态，观测到的可能性
• P(状态 | 观测) 是「后验」——结合观测后的新信念

# 贝叶斯更新示例（简化版）
def bayesian_update(prior_mean, prior_var, measurement, meas_var):
    # 后验均值 = 先验与观测的加权平均
    posterior_mean = (prior_mean * meas_var + measurement * prior_var) / (prior_var + meas_var)
    # 后验方差 = 调和平均
    posterior_var = 1 / (1/prior_var + 1/meas_var)
    return posterior_mean, posterior_var

# 初始信念：位置在 10m，不确定性方差 4
prior_mean, prior_var = 10.0, 4.0

# 观测到位置 12m，传感器方差 1
post_mean, post_var = bayesian_update(prior_mean, prior_var, 12.0, 1.0)

print(f"更新后位置: {post_mean:.2f}m, 不确定性: {post_var:.2f}")

嗯，这里要注意：贝叶斯更新后，不确定性一定是降低的（除非观测方差无穷大）。我建议你在做决策算法时，始终保留一个不确定性估计——它比均值更重要。

知识体系总览

下面这张图，是我自己梳理的本章知识结构。你看一眼就能明白各部分之间的关系：

这张图把四个模块串起来了。你看，向量和矩阵处理「状态是什么」，概率论处理「状态有多不确定」，贝叶斯定理则把两者结合——用新观测不断修正对状态的认知。最终，这些数学工具都服务于决策算法与速度约束融合这个核心目标。

我个人觉得，理解这张图比背公式更重要。公式忘了可以查，但知识体系乱了，后面学起来就费劲了。

可可精品资料库