上传者: Twilight737
|
上传时间: 2021-11-30 17:13:41
|
文件大小: 161KB
|
文件类型: ZIP
卡尔曼滤波器原理实现:
X(k|k-1) = AX(k-1|k-1) + BU(k)
P(k|k-1) = AP(k-1|k-1)A.T + Q
K(k) = P(k|k-1)H.T / (HP(k|k-1)H.T + R)
X(k|k) = X(k|k-1) + K(k)[Z(k) - HX(k|k-1)]
P(k|k) = [1 - K(k)H]P(k|k-1)
第1步:计算中间量X(k|k-1)、P(k|k-1)。
第2步:计算卡尔曼增益K(k)。
第3步:计算当前时刻的状态最佳估计:X(k|k)、P(k|k)。