卡尔曼预测更新
predict：
x
^
i
−
=
A
x
^
i
−
1
+
B
u
i
−
1
\widehat{x}_i^{-} = A\widehat{x}_{i-1}+Bu_{i-1}
x
i−=Ax
i−1+Bui−1
correction:
x
^
i
=
x
^
i
−
+
k
(
y
i
−
H
x
^
i
−
)
\widehat{x}_i = \widehat{x}_i^{-}+k(y_i-H\widehat{x}_i^{-})
x
i=x
i−+k(yi−Hx
i−)
其中， k k k为kalman gain matrix; y i y_i yi观测值; A A A状态转移矩阵; H H H测量矩阵; B控制矩阵。
计算预测值、预测值和真实值之间的误差协防差矩阵
x ^ k − = A x ^ k − 1 − + B u k − 1 \widehat{x}_k^{-} = A\widehat{x}_{k-1}^{-}+Bu_{k-1} x k−=Ax k−1−+Buk−1
P k − = A P k − 1 A T + Q P_k^{-}=AP_{k-1}A^T+Q Pk−=APk−1AT+Q
Q Q Q系统误差
计算卡尔曼增益 k k k，再得到估计值
K k = P k − H T ( H P K − H T + R ) − 1 K_k=P_k^{-}H^T(HP_K^{-}H^T+R)^{-1} Kk=Pk−HT(HPK−HT+R)−1
x
^
k
=
A
x
^
k
−
+
K
k
(
Z
k
−
H
x
^
k
−
)
\widehat{x}_k = A\widehat{x}_k^{-}+K_k(Z_k-H\widehat{x}_k^{-})
x
k=Ax
k−+Kk(Zk−Hx
k−)
R
R
R测量误差
计算估计值和真实值之间的误差协防差矩阵
P k = ( 1 − K k H ) P k − P_k=(1-K_kH)P_k^{-} Pk=(1−KkH)Pk−
卡尔曼转换关系
估计值==》预测值==》估计值==》
…
\ldots
…估计值==》预测值==》估计值==》
…
\ldots
…
核心在于计算卡尔慢增益
k
k
k
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
