卡尔曼跟踪

卡尔曼预测更新

predict:
x^i−=Ax^i−1+Bui−1\widehat{x}_i^{-} = A\widehat{x}_{i-1}+Bu_{i-1}xi=Axi1+Bui1

correction:
x^i=x^i−+k(yi−Hx^i−)\widehat{x}_i = \widehat{x}_i^{-}+k(y_i-H\widehat{x}_i^{-})xi=xi+k(yiHxi)

其中,kkk为kalman gain matrix; yiy_iyi观测值; AAA状态转移矩阵; HHH测量矩阵; B控制矩阵。

计算预测值、预测值和真实值之间的误差协防差矩阵

x^k−=Ax^k−1−+Buk−1\widehat{x}_k^{-} = A\widehat{x}_{k-1}^{-}+Bu_{k-1}xk=Axk1+Buk1

Pk−=APk−1AT+QP_k^{-}=AP_{k-1}A^T+QPk=APk1AT+Q

QQQ系统误差

计算卡尔曼增益kkk,再得到估计值

Kk=Pk−HT(HPK−HT+R)−1K_k=P_k^{-}H^T(HP_K^{-}H^T+R)^{-1}Kk=PkHT(HPKHT+R)1

x^k=Ax^k−+Kk(Zk−Hx^k−)\widehat{x}_k = A\widehat{x}_k^{-}+K_k(Z_k-H\widehat{x}_k^{-})xk=Axk+Kk(ZkHxk)
RRR测量误差

计算估计值和真实值之间的误差协防差矩阵

Pk=(1−KkH)Pk−P_k=(1-K_kH)P_k^{-}Pk=(1KkH)Pk

卡尔曼转换关系

估计值==》预测值==》估计值==》…\ldots估计值==》预测值==》估计值==》…\ldots
核心在于计算卡尔慢增益kkk

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值