状 态 ： x ⃗ k = F k x ⃗ k − 1 + B k u ⃗ k + N o i s e ; 状 态 方 差 ： P k = F k P k − 1 F k T + Q p e r i o d ; 根 据 预 测 的 状 态 反 推 的 传 感 器 读 数 ： z ⃗ k ( e x p e c t e d ) = H k x k ⃗ ; 反 推 的 传 感 器 读 数 方 差 ： R k ( e x p e c t e d ) = H k P k H k T ; 有 z ⃗ k ( s e n s o r ) ∼ N ( μ z k ( s e n s o r ) , R s e n s o r ) ; H k x ⃗ k ( o p t i m a l ) = H k x ⃗ k + H k P k H k T H k P k H k T + R s e n s o r ( z ⃗ k − H k x ⃗ k ) ; x ⃗ k ( o p t i m a l ) = x ⃗ k + P k H k T H k P k H k T + R s e n s o r ( z ⃗ k − H k x ⃗ k ) ; H k P k ( o p t i m a l ) H k T = H k P k H k T − H k P k H k T H k P k H k T + R s e n s o r H k P k H k T ; 则 P k ( o p t i m a l ) = P k − P k H k T H k P k H k T + R s e n s o r H k P k ; 若 定 义 ： 卡 尔 曼 增 益 ： K = H k P k H k T H k P k H k T + R 则 有 ： x ⃗ k ( o p t i m a l ) = x ⃗ k + H k − 1 K ( z ⃗ k − H k x ⃗ k ) ; P k ( o p t i m a l ) = P k ( E − H k − 1 K H k ) ; ⋅ ⋅ ⋅ ⋅ ⋅ 综 上 所 得 ： x ⃗ k = F k x ⃗ k − 1 ( o p t i m a l ) + B k u ⃗ k ; P k = F k P k − 1 ( o p t i m a l ) F k T + Q p e r i o d ; K = H k P k H k T H k P k H k T + R ; x ⃗ k ( o p t i m a l ) = x ⃗ k + H k − 1 K ( z ⃗ k − H k x ⃗ k ) ; P k ( o p t i m a l ) = P k ( E − H k − 1 K H k ) ; 状态：\vec{x}_k=F_k\vec{x}_{k-1}+B_k\vec{u}_k+Noise;\\ 状态方差：P_k=F_kP_{k-1}F^T_k+Q_{period};\\ \\ 根据预测的状态反推的传感器读数：\\\vec{z}_{k(expected)}=H_k\vec{x_k}; \\ 反推的传感器读数方差：\\R_{k(expected)}=H_kP_kH^T_k;\\ 有\vec{z}_{k(sensor)} \sim N(\mu_{z_{k(sensor)}},R_{sensor});\\ H_k\vec{x}_{k(optimal)}=H_k\vec{x}_k+\frac{H_kP_kH^T_k}{H_kP_kH^T_k+R_{sensor}}(\vec{z}_k-H_k\vec{x}_k);\\ \vec{x}_{k(optimal)}=\vec{x}_k+\frac{P_kH^T_k}{H_kP_kH^T_k+R_{sensor}}(\vec{z}_k-H_k\vec{x}_k);\\ H_kP_{k(optimal)}H^T_k=H_kP_kH^T_k-\frac{H_kP_kH^T_k}{H_kP_kH^T_k+R_{sensor}}H_kP_kH^T_k;\\ 则P_{k(optimal)}=P_k-\frac{P_kH^T_k}{H_kP_kH^T_k+R_{sensor}}H_kP_k;\\ 若定义：\\ 卡尔曼增益：K=\frac{H_kP_kH^T_k}{H_kP_kH^T_k+R}\\ 则有：\vec{x}_{k(optimal)}=\vec{x}_k+H^{-1}_kK(\vec{z}_k-H_k\vec{x}_k);\\ P_{k(optimal)}=P_k(E-H^{-1}_kKH_k);\\ ·\\·\\·\\·\\·\\ 综上所得：\\ \vec{x}_k=F_k\vec{x}_{k-1(optimal)}+B_k\vec{u}_k;\\ P_k=F_kP_{k-1(optimal)}F^T_k+Q_{period};\\ K=\frac{H_kP_kH^T_k}{H_kP_kH^T_k+R};\\ \vec{x}_{k(optimal)}=\vec{x}_k+H^{-1}_kK(\vec{z}_k-H_k\vec{x}_k);\\ P_{k(optimal)}=P_k(E-H^{-1}_kKH_k);\\ 状态：xk=Fkxk−1+Bkuk+Noise;状态方差：Pk=FkPk−1FkT+Qperiod;根据预测的状态反推的传感器读数：zk(expected)=Hkxk;反推的传感器读数方差：Rk(expected)=HkPkHkT;有zk(sensor)∼N(μzk(sensor),Rsensor);Hkxk(optimal)=Hkxk+HkPkHkT+RsensorHkPkHkT(zk−Hkxk);xk(optimal)=xk+HkPkHkT+RsensorPkHkT(zk−Hkxk);HkPk(optimal)HkT=HkPkHkT−HkPkHkT+RsensorHkPkHkTHkPkHkT;则Pk(optimal)=Pk−HkPkHkT+RsensorPkHkTHkPk;若定义：卡尔曼增益：K=HkPkHkT+RHkPkHkT则有：xk(optimal)=xk+Hk−1K(zk−Hkxk);Pk(optimal)=Pk(E−Hk−1KHk);⋅⋅⋅⋅⋅综上所得：xk=Fkxk−1(optimal)+Bkuk;Pk=FkPk−1(optimal)FkT+Qperiod;K=HkPkHkT+RHkPkHkT;xk(optimal)=xk+Hk−1K(zk−Hkxk);Pk(optimal)=Pk(E−Hk−1KHk);
扫一扫
评论
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
查看更多评论
添加红包
