前言
在了解卡尔曼滤波前我们先来看看完整的卡尔曼滤波公式。
模型建立：
状态方程：X_t=A*X_t-1+B*u_t+W_t
观测方程：Y_t = C*X_t+V_t
W_t→N(0:Q_t)
V_t→N(0:R_t)
其中：Y_t=z_t，A=F_t，C=H_t
卡尔曼公式：
预测：
X_t~ =F_t * X_t-1 + B_t * u_t
P_t~ = F_t * P_t-1 * F_t^T + Q_t
更新：
K_t = P_t~ * H_t^T * ( H_t * P_t~ * H_t^T + R_t)^(-1)
X_t = X_t~ + K_t*(z_t - H_t * X_t~ )
P_t = ( I - K_t * H_t) * P_t~
它还有另一种表现形式
模型建立：
状态方程：X_t=A*X_t-1+B*u_t+W_t
观测方程：Y_t = C*X_t+V_t
W_t→N(0:Q_t)
V_t→N(0:R_t)
其中：Y_t=z_t，A=F_t，C=H_t
卡尔曼公式：
预测：
X_t~ =F_t * X_t-1 + B_t * u_t
P_t~ = F_t * P_t-1 * F_t^T + Q_t
更新：
Y = z_t - H_t * X_t~
S = H_t * P_t~ * H_t^T + R_t
K_t = P_t~ * H_t^T * S^(-1)
X_t = X_t~ + K_t * Y
P_t = ( I - K_t * H_t) * P_t~
这里我们可以看到它们的更新公式其实是一样的，只是第二种加入了Y、S为中间变量
第一种的更新：
K_t = P_t~ * H_t^T * ( H_t * P_t~ * H_t^T + R_t)^(-1)
X_t = X_t~ + K_t*(z_t - H_t * X_t~ )
P_t = ( I - K_t * H_t) * P_t~
第二种的更新：
Y = z_t - H_t * X_t~
S = H_t * P_t~ * H_t^T + R_t
K_t = P_t~ * H_t^T * S^(-1)
X_t = X_t~ + K_t * Y
P_t = ( I - K_t * H_t) * P_t~
卡尔曼公式解析
理解卡尔曼公式可以分为两个步骤：一、了解每个符号的意义。二、理解每行公式的思想。
公式符号
我们先来看看完整公式中有哪些符号
模型建立：
状态方程：X_t=A*X_t-1+B*u_t+W_t
观测方程：Y_t = C*X_t+V_t
W_t→N(0:Q_t)
V_t→N(0:R_t)
其中：Y_t=z_t，A=F_t，C=H_t
预测：
X_t~ =F_t * X_t-1 + B_t * u_t
P_t~ = F_t * P_t-1 * F_t^T + Q_t
更新：
K_t = P_t~ * H_t^T * ( H_t * P_t~ * H_t^T + R_t)^(-1)
X_t = X_t~ + K_t*(z_t - H_t * X_t~ )
P_t = ( I - K_t * H_t) * P_t~
公式符号：
t:代表当前时刻
t-1:代表上一时刻
X_t：代表x的当前时刻，同理X_t-1：代表x的上一时刻，其他的一样
X_t~：代表X_t的先验估计，可以理解为X_t更新时产生的中间变量，P_t~同理
X：状态向量，是我们需要处理的数据集合，以向量形式出现。例如：[px位置,py位置,vx速度,vy速度]
P：X状态向量的协方差矩阵，用于描述X状态向量中每个元素对其他元素的影响，由协方差与方差组成的矩阵。
K：卡尔曼增益。卡尔曼滤波中的关键矩阵，是平衡状态方程与观测方程对X状态向量的影响的，它本质上就是一个权重因子。可以类比为互补滤波中的系数
F：状态转移矩阵，用于描述x状态向量丛上一时刻在非输入情况下到当前时刻的变化。例如，电容在充满电后空置它，电荷量会逐渐减小：汽车在下坡时速度自然增快
B：输入控制矩阵，用于描述u_t输入是如何影响状态向量的。例如，当我们控制小车前进指定距离时，我们实际控制的是电机旋转的角度，而我们关注的量其实是距离，那么我们就要将角度转换成圈数在乘上车轮的周长
u：输入向量，系统实际上的输入量。在特定情况下没有输入量或是将输入量并入噪声中。
H：观测矩阵，用于描述状态向量与观测量(传感器检测的量)之间的转换关系，为后面求K做准备
z：观测向量，其实就是由传感器检测的量组成的向量
W_t：状态方程中的的过程噪声，用于描述理论模型与实际模型的差
V_t：观测方程中的的观测噪声，用于描述传感器与实际模型的差
Q_t：W_t的协方差矩阵
R_t：V_t的协方差矩阵
I：单位矩阵，大小为 状态向量个数X状态向量个数 运算用的辅助矩阵
公式理解
模型建立：
状态方程：X_t=A*X_t-1+B*u_t+W_t
观测方程：Y_t = C*X_t+V_t
W_t→N(0:Q_t)
V_t→N(0:R_t)
其中：Y_t=z_t，A=F_t，C=H_t
卡尔曼滤波公式：
预测：
X_t~ =F_t * X_t-1 + B_t * u_t
P_t~ = F_t * P_t-1 * F_t^T + Q_t
更新：
K_t = P_t~ * H_t^T * ( H_t * P_t~ * H_t^T + R_t)^(-1)
X_t = X_t~ + K_t*(z_t - H_t * X_t~ )
P_t = ( I - K_t * H_t) * P_t~
模型方程
模型建立：
状态方程：X_t=A*X_t-1+B*u_t+W_t
观测方程：Y_t = C*X_t+V_t
W_t→N(0:Q_t)
V_t→N(0:R_t)
其中：Y_t=z_t，A=F_t，C=H_t
这里的状态方程很好理解，当前状态=经过特定变化的上一时刻状态+经过特定变化的当前时刻输入+一些噪声误差。
观测方程理解是与状态方程不同，这里的观测方程其实叫观测值公式比较好理解，即传感器测量值=经过特定变化的当前时刻状态+传感器与实际值的噪声误差
理解这些方程并不困难，我们仔细观察可以发现，模型建立几乎包含了卡尔曼公式中的所有参数，所以我们的重点是如何将我们用到的物理模型转换成状态方程与观测方程
例：匀速运动的一维小车
1、物理模型分析
- 我们关心的状态：小车的位置 p 和速度 v。
- 运动模型：当前时刻位置 = 上一时刻位置 + 速度 * 时间间隔。
- 观测模型：GPS传感器，每秒告诉我们小车的位置，但这个测量值存在误差。
2、数学模型建立
- 确定状态向量：X=[ P , V ]^T
- 确定观测向量：Z=[ P ]
- 构建状态方程：
状态方程：X_t=F_t*X_t-1+B*u_t+W_t
实际状态变化：[Pt,Vt]^T=[f00,f01;f10,f11]^T*[Pt-1,Vt-1]^T(因为没有输入所以B、u不存在)
状态向量的运动公式：
pt=pt−1+vt−1∗Δt
vt=vt−1 (假设匀速)
所以Ft=[1,Δt;0,1]
- 构建观测方程：
观测方程：Y_t = H*X_t+V_t
实际观测变化:[Pz]=[h00,h01]^T*[Pt,Vt]^T
所以H=[1,0]
3、噪声协方差矩阵
- 构建R_T：R_T大小：观测向量个数X观测向量个数。这里的观测向量个数为1，所以R_T简化为标准值的方差。这个值可以从传感器的数据手册或通过静态测试得到，假设其读数误差的标准差约为 1.5 米则Rt=[σ^2]=[(1.5)^2]=[2.25]
- 构建Q_T：Q_T大小：状态向量个数X状态向量个数。这个数值表示的是我们对模型的信任程度，它类似于PID控制中的P、I、D值，需要我们根据模型对理解和调试来构建。
我们设过程噪声W_t是受到加速度a的影响且a符合高斯分布
对位置的影响：Δp=0.5 * Δt^2 * a
对速度的影响：Δv=Δt * a
则W_t=[0.5 * Δt^2 , Δt] * a
Q_T=E[W_t*W_t^t]
= E[[½Δt²; Δt] · a · aᵀ · [½Δt², Δt]]
= [½Δt²; Δt] · E[a²] · [½Δt², Δt]
= σ_a² · [½Δt²; Δt] · [½Δt², Δt]
= σ_a² · [(¼Δt⁴) (½Δt³); (½Δt³) (Δt²)]
可以看到当我们的Δt较小(0.01~0.001)时其矩阵的元素是趋近0的，这时我们可以根据它的物理意义与实际表现来调整它。
卡尔曼公式
模型建立：
状态方程：X_t=A*X_t-1+B*u_t+W_t
观测方程：Y_t = C*X_t+V_t
W_t→N(0:Q_t)
V_t→N(0:R_t)
其中：Y_t=z_t，A=F_t，C=H_t
卡尔曼公式：
预测：
X_t~ =F_t * X_t-1 + B_t * u_t
P_t~ = F_t * P_t-1 * F_t^T + Q_t
更新：
K_t = P_t~ * H_t^T * ( H_t * P_t~ * H_t^T + R_t)^(-1)
X_t = X_t~ + K_t*(z_t - H_t * X_t~ )
P_t = ( I - K_t * H_t) * P_t~
X_t~ =F_t * X_t-1 + B_t * u_t：(纯理论)先验状态计算，这是基于系统模型的纯理论预测，不考虑当前时刻的实际观测数据。运算过程与状态方程一样，但是消除了过程噪声W_t，过程噪声体现在P_t~
P_t~ = F_t * P_t-1 * F_t^T + Q_t：先验状态协方差矩阵计算，预测状态估计的不确定性的变化， 加上过程噪声Q_t体现状态方程中W_t的变化。
K_t = P_t~ * H_t^T * ( H_t * P_t~ * H_t^T + R_t)^(-1)：计算卡尔曼增益。这个公式的本质是将预测量的高斯分布与检测量的高斯分布相乘，找出它们共同的高斯分布。可以理解为在预测不确定性(P_t~)和观测不确定性(R_t)之间找到最优平衡点这个公式实际上是通过数学推导得出的最优解，一个固定的公式记住就可以了。
X_t = X_t~ + K_t * (z_t - H_t * X_t~)：状态向量更新，
- z_t - H_t * X_t~：观测残差，这个是经过观测矩阵处理的理论值与传感器测量值的差，是卡尔曼的观测方程对状态方程的修正修正量。
- K_t * (z_t - H_t * X_t~)：使用卡尔曼增益将观测残差映射到状态向量更新
P_t = ( I - K_t * H_t) * P_t~：更新协方差矩阵，使用卡尔曼增益将观测详细加入到新的协方差矩阵中
扩展卡尔曼滤波
标准卡尔曼滤波的局限性
在标准卡尔曼滤波中，卡尔曼的数学模型必须是线性的。其具体表现在：
- 状态方程：X_t=F_t * X_t-1 + B * u_t + W_t，上一时刻状态于当前时刻状态是线性变化，输入量可以线性变化的到状态量。
- 观测方程：Y_t = H * X_t+V_t，状态量与观测量是线性关系
然而，现实世界中的绝大多数系统都是非线性的：
- 涉及到三角函数
- 坐标系转换
- 平方与开平方
- 指数和对数运算
当进行非线性转换时高斯分布特性将不再保持，因此我们的问题就变成了如何在非线性系统中保持卡尔曼滤波的特性？
扩展卡尔曼中的数学
想要解决标准卡尔曼的非线性问题，我们可以参考数学中泰勒展开的核心思想：局部线性化
什么是局部线性化？在一个非常小的区域内，用一条直线（或一个平面）去近似一条复杂的曲线（或曲面）的过程就是局部线性化。
局部线性化直观理解
- 在弯曲的山路上，虽然整条路是弯的，但在每一个瞬间我们都可以近似为直线行驶
- 地球是球形的，但在小范围内我们可以把它当作平面
局部线性化在扩展卡尔曼滤波中则体现为在系统当前的最佳估计点（均值点）附近，用线性系统来代替非线性系统。如果需要实现它，我们需要用到两个数学工具：一阶泰勒展开、雅可比矩阵。
一阶泰勒展开
当非线性函数 f(x)，在点 x -> a处需要进行局部线性化时我们可以使用一阶泰勒展开公式：
f(x) ≈ f(a) + f'(a)(x - a)
或是
f(x) ≈ f(a) + ∂f/∂x|_a * (x - a)
对于多变量函数f(x, y, z)在点f(a, b, c)同样可以使用一阶一阶泰勒展开公式：
f(x, y, z) ≈ f(a, b, c) + ∂f/∂x |(a,b,c) * (x - a) + ∂f/∂y |(a,b,c) * (y - b) + ∂f/∂z |_(a,b,c) * (z - c)
∂f/∂x：f(x, y, z)关于x的偏导
∂f/∂y：f(x, y, z)关于y的偏导
∂f/∂z：f(x, y, z)关于z的偏导
雅可比矩阵
雅可比矩阵：由多变量函数的一阶偏导数构成的矩阵，它代表了函数在某个点的最佳线性近似。
在扩展卡尔曼中，状态方程通常时由多个多变量函数组成，将其局部线性化就雅可比矩阵。
[ ∂f₁/∂x₁ ∂f₁/∂x₂ ... ∂f₁/∂xₙ ]
J_f = [ ∂f₂/∂x₁ ∂f₂/∂x₂ ... ∂f₂/∂xₙ ]
[ ... ... ... ... ]
[ ∂fₘ/∂x₁ ∂fₘ/∂x₂ ... ∂fₘ/∂xₙ ]
∂f₁/∂x、 ∂f₁/∂x₂、∂fₘ/∂xₙ都是偏导数
扩展卡尔曼公式
模型建立：
状态方程：X_t = f(X_t-1, u_t, W_t)
观测方程：Y_t = h(X_t, V_t)
-------------------------------------------
在最优估计点 X_t-1^ & W_t=0处对状态方程进行一阶泰勒展开：
X_t ≈ f(X_t-1^, u_t, 0) + F_t * (X_t-1 - X_t-1^) + L_t * W_t
其中：
F_t = ∂f/∂X |_{X=X_t-1} （状态转移雅可比矩阵）
L_t = ∂f/∂W |_{W=0} （过程噪声雅可比矩阵）
在预测状态 X_t~ & V_t=0处对观测方程进行一阶泰勒展开：
Y_t ≈ h(X_t~, 0) + H_t * (X_t - X_t~) + M_t * V_t
其中：
H_t = ∂h/∂X |_{X=X_t~} （观测雅可比矩阵）
M_t = ∂h/∂V |_{V=0} （观测噪声雅可比矩阵）
--------------------------------------------
W_t → N(0:Q_t)
V_t → N(0:R_t)
其中：Y_t = z_t
扩展卡尔曼滤波：
预测：
X_t~ = f(X_t-1, u_t, 0)
P_t~ = F_t * P_t-1 * F_t^T + L_t * Q_t * L_t^T
更新：
K_t = P_t~ * H_tᵀ * (H_t * P_t~ * H_t^T + M_t * R_t * M_t^T)^(-1)
X_t = X_t~ + K_t * (z_t - h(X_t~, 0))
P_t = (I - K_t * H_t) * P_t~
--------------------------------------------
如果噪声是加性的:
状态方程：X_t = f(Xₜ₋₁, u_t) + W_t
观测方程：Y_t = h(X_t) + V_t
那么L_t = I、M_t = I
则
P_t~ = F_t * P_t-1 * F_t^T + Q_t
K_t = P_t~ * H_tᵀ * (H_t * P_t~ * H_t^T + R_t)^(-1)
扩展卡尔曼公式变化
对比与普通卡尔曼滤波公式最大的变化有四处：
- 状态方程：X_t = f(Xₜ₋₁, u_t,W_t) ,X_t=A * X_t-1+ B * u_t + W_t 这两个方程其实是等价的，X_t = f(Xₜ₋₁, u_t,W_t)是X_t=A * X_t-1+ B * u_t + W_t的非线性版本
- 观测方程：Y_t = h(X_t,V_t),Y_t = C * X_t+V_t，观测方程与状态方程相同，两个方程也是等价的，非线性版本的。
- L_t * Q_t * L_t^T：这是噪声在非线性变化中的局部线性化方法。这是过程噪声在状态空间中的有效协方差矩阵
Cov[L_t * W_t] = E[(L_t * W_t)(L_t * W_t)^T]
= L_t * E[W_t * W_t^T] * L_t^T
= L_t * Q_t * L_t^T
- M_t * R_t * M_t^T：与过程噪声相同，是局部线性化方法。这是观测噪声在观测空间中的有效协方差矩阵
Cov[M_t * V_t] = E[(M_t * V_t)(M_t * V_t)^T]
= M_t * E[V_t * V_t^T] * M_t^T
= M_t * R_t * M_t^T
公式符号
这里的符号与标准卡尔曼滤波基本是一样的，只是由普通的矩阵变为了雅可比矩阵
f ( · )：非线性状态转移函数
h ( · )：非线性观测函数
Q_t：过程噪声协方差矩阵
R_t：观测噪声协方差矩阵
F_t：状态转移雅可比矩阵，F_t = ∂f/∂X |_{X=X_t-1}
L_t：过程噪声雅可比矩阵，L_t = ∂f/∂W |_{W=0}
H_t：观测雅可比矩阵，H_t = ∂h/∂X |_{X=X_t~}
M_t：观测噪声雅可比矩阵，M_t = ∂h/∂V |_{V=0}
模型建立
模型建立：
状态方程：X_t = f(X_t-1, u_t, W_t)
观测方程：Y_t = h(X_t, V_t)
-------------------------------------------
在最优估计点 X_t-1^ & W_t=0处对状态方程进行一阶泰勒展开：
X_t ≈ f(X_t-1^, u_t, 0) + F_t * (X_t-1 - X_t-1^) + L_t * W_t
其中：
F_t = ∂f/∂X |_{X=X_t-1} （状态转移雅可比矩阵）
L_t = ∂f/∂W |_{W=0} （过程噪声雅可比矩阵）
在预测状态 X_t~ & V_t=0处对观测方程进行一阶泰勒展开：
Y_t ≈ h(X_t~, 0) + H_t * (X_t - X_t~) + M_t * V_t
其中：
H_t = ∂h/∂X |_{X=X_t~} （观测雅可比矩阵）
M_t = ∂h/∂V |_{V=0} （观测噪声雅可比矩阵）
--------------------------------------------
这里的状态方程与观测方程与标准卡尔曼公式的意义是一样的，唯一的区别是将使用矩阵的线性转换改成了使用函数的非线性转换。我们的重点依旧是如何将我们用到的物理模型转换成状态方程与观测方程
例子1：单摆系统（机械振动模型）
模型描述
- 状态：角度θ，角速度ω
- 控制输入：无（自由振动）
- 观测：角度
状态方程：
X_t = [θ_t, ω_t]^T
f(X_t-1, 0, W_t) = [θ_t-1 + ω_t-1·Δt,
ω_t-1 - (g/L)·sin(θ_t-1)·Δt] + W_t]
W_t噪声没有参与复杂的运算，是直接加在到状态转移函数的结果上的，因此是加性噪声。
观测方程：
Z_t=[θ_t]
Z_t = h(X_t, V_t) = θ_t + V_t
雅可比矩阵计算：
F_t = ∂f/∂X = [1, Δt;
-(g/L)·cos(θ_t-1)·Δt, 1]
L_t = ∂f/∂W = I (2×2单位矩阵，因为噪声是加性的)
H_t = ∂h/∂X = [1, 0] (只观测角度)
M_t = ∂h/∂V = 1
对应公式：
预测：
X_t~ = [θ_t-1 + ω_t-1·Δt, ω_t-1 - (g/L)·sin(θ_t-1)·Δt]^T
P_t~ = F_t · P_t-1 · F_t^T + Q_t
更新：
K_t = P_t~ · H_t^T · (H_t · P_t~ · H_t^T + R_t)^(-1)
X_t = X_t~ + K_t · (z_t - θ_t~)
P_t = (I - K_t · H_t) · P_t~
例子2：自行车（二维平面运动）
模型描述:
- 状态：位置(x,y)，航向角θ，速度v
- 控制输入：加速度a，转向率ω
- 观测：GPS位置(x,y)
状态向量：
X_t = [x, y, θ, v]^T
输入向量：
u_t = [a, ω]^T
状态向量与输入向量与噪声的公式：
x_t = x_{t-1} + v_{t-1}·cos(θ_{t-1})·Δt
y_t = y_{t-1} + v_{t-1}·sin(θ_{t-1})·Δt
θ_t = θ_{t-1} + ω_t·Δt//ω_t 是控制输入
v_t = v_{t-1} + a_t·Δt + W_t
状态方程：
X_t = [x, y, θ, v]^T
f(X_{t-1}, u_t, W_t) =
[x_{t-1} + v_{t-1}·cos(θ_{t-1})·Δt,
y_{t-1} + v_{t-1}·sin(θ_{t-1})·Δt,
θ_{t-1} + ω_t·Δt,
v_{t-1} + a_t·Δt + W_t]
观测模型：
Y_t = h(X_t, V_t) = [x_t, y_t]^T + V_t
状态转移雅可比矩阵推导：
F_t = ∂f/∂X =
[∂f₁/∂x, ∂f₁/∂y, ∂f₁/∂θ, ∂f₁/∂v;
∂f₂/∂x, ∂f₂/∂y, ∂f₂/∂θ, ∂f₂/∂v;
∂f₃/∂x, ∂f₃/∂y, ∂f₃/∂θ, ∂f₃/∂v;
∂f₄/∂x, ∂f₄/∂y, ∂f₄/∂θ, ∂f₄/∂v]
逐个计算：
∂f₁/∂x = 1
∂f₁/∂y = 0
∂f₁/∂θ = -v_{t-1}·sin(θ_{t-1})·Δt
∂f₁/∂v = cos(θ_{t-1})·Δt
∂f₂/∂x = 0
∂f₂/∂y = 1
∂f₂/∂θ = v_{t-1}·cos(θ_{t-1})·Δt
∂f₂/∂v = sin(θ_{t-1})·Δt
∂f₃/∂x = 0
∂f₃/∂y = 0
∂f₃/∂θ = 1
∂f₃/∂v = 0
∂f₄/∂x = 0
∂f₄/∂y = 0
∂f₄/∂θ = 0
∂f₄/∂v = 1
最终F_T
F_t = [1, 0, -v·sin(θ)·Δt, cos(θ)·Δt;
0, 1, v·cos(θ)·Δt, sin(θ)·Δt;
0, 0, 1, 0;
0, 0, 0, 1]
过程噪声雅可比矩阵 L_t
L_t = ∂f/∂W = [0, 0, 0, 1]^T
观测雅可比矩阵 H_t
H_t = ∂h/∂X =
[∂h₁/∂x, ∂h₁/∂y, ∂h₁/∂θ, ∂h₁/∂v;
∂h₂/∂x, ∂h₂/∂y, ∂h₂/∂θ, ∂h₂/∂v]
∂h₁/∂x = 1, ∂h₁/∂y = 0, ∂h₁/∂θ = 0, ∂h₁/∂v = 0
∂h₂/∂x = 0, ∂h₂/∂y = 1, ∂h₂/∂θ = 0, ∂h₂/∂v = 0
H_t = [1, 0, 0, 0;
0, 1, 0, 0]
观测噪声雅可比矩阵 M_t
由于观测噪声是加性的
M_t = ∂h/∂V = I₂ (2×2单位矩阵)
完整公式：
# 状态预测
X_t~ = [x_{t-1} + v_{t-1}·cos(θ_{t-1})·Δt,
y_{t-1} + v_{t-1}·sin(θ_{t-1})·Δt,
θ_{t-1} + ω_t·Δt,
v_{t-1} + a_t·Δt]^T
# 协方差预测
P_t~ = F_t · P_{t-1} · F_t^T + L_t · Q_t · L_t^T
# 卡尔曼增益
S_t = H_t · P_t~ · H_t^T + M_t · R_t · M_t^T
K_t = P_t~ · H_t^T · S_t^(-1)
# 状态更新
z_t = 实际GPS测量值
innovation = z_t - [x_t~, y_t~]^T
X_t = X_t~ + K_t · innovation
# 协方差更新
P_t = (I - K_t · H_t) · P_t~
公式分析
扩展卡尔曼滤波的公式意义与标准卡尔曼公式的意义是一样的。
卡尔曼滤波初始值确定
初始状态向量的确定方法
-
直接测量法：直接测量或准确知道系统的初始状态
- 适用场景：系统启动时有准确的传感器测量
- 示例：GPS定位系统开机时有准确的初始位置
-
首次观测法：用第一次观测值初始化可直接观测的状态分量
- 适用场景：部分状态可直接观测
- 示例：用第一次GPS测量初始化位置状态
-
零值初始化法
- 适用场景：对初始状态完全未知
- 风险：如果实际状态不为零，收敛时间较长
-
物理模型推断法
- 适用场景：可以通过物理关系推导初始状态
- 示例：静止物体初始速度为零
-
多传感器融合法（建议补充）
- 适用场景：有多个传感器可用
- 示例：结合IMU和GPS数据计算初始姿态
初始协方差矩阵的确定方法
- 置信度反映法
高置信度 → P 小：P = diag([0.1, 0.1, ...])
低置信度 → P 大：P = diag([1000, 1000, ...])
说明：diag函数用于构造对角矩阵，对角线元素为各状态分量的方差
- 经验对角矩阵法
通用公式：P= diag(σ1^2, σ2^2, ..., σn^2)
px,py位置不确定性：±10米 → 方差 = 100
vx,vy速度不确定性：±5m/s → 方差 = 25
P₀ = [100, 0, 0, 0; 0, 100, 0, 0; 0, 0, 25, 0; 0, 0, 0, 25]
