kalman滤波纯纯纯纯纯纯理论推导

状态：x⃗k=Fkx⃗k−1+Bku⃗k+Noise;状态方差：Pk=FkPk−1FkT+Qperiod;根据预测的状态反推的传感器读数：z⃗k(expected)=Hkxk⃗;反推的传感器读数方差：Rk(expected)=HkPkHkT;有z⃗k(sensor)∼N(μzk(sensor),Rsensor);Hkx⃗k(optimal)=Hkx⃗k+HkPkHkTHkPkHkT+Rsensor(z⃗k−Hkx⃗k);x⃗k(optimal)=x⃗k+PkHkTHkPkHkT+Rsensor(z⃗k−Hkx⃗k);HkPk(optimal)HkT=HkPkHkT−HkPkHkTHkPkHkT+RsensorHkPkHkT;则Pk(optimal)=Pk−PkHkTHkPkHkT+RsensorHkPk;若定义：卡尔曼增益：K=HkPkHkTHkPkHkT+R则有：x⃗k(optimal)=x⃗k+Hk−1K(z⃗k−Hkx⃗k);Pk(optimal)=Pk(E−Hk−1KHk);⋅⋅⋅⋅⋅综上所得：x⃗k=Fkx⃗k−1(optimal)+Bku⃗k;Pk=FkPk−1(optimal)FkT+Qperiod;K=HkPkHkTHkPkHkT+R;x⃗k(optimal)=x⃗k+Hk−1K(z⃗k−Hkx⃗k);Pk(optimal)=Pk(E−Hk−1KHk);状态：\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);

kalman滤波纯纯纯纯纯纯理论推导相关推荐

Kalman滤波算法详细推导及简单匀速直线运动程序仿真（matlab）
Kalman滤波算法详细推导及简单匀速直线运动程序仿真(matlab) 起初只是知道Kalman滤波的核心公式和会用,没有仔细研究,最近老师让讲Kalman算法,所以系统的学习了该算法,并结合匀速直线 ...
kalman滤波推导
线性卡尔曼滤波卡尔曼滤波在温度测量中的应用 X(k)=A*X(k-1)+T*W(k-1) Z(k)=H*X(k)+V(k) 房间温度在25摄氏度左右,测量误差为正负0.5摄氏度,方差0.25,R=0 ...
MSCKF理论推导与代码解析
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达在SLAM后端中,主要有两种主流方法用于优化:基于滤波的方法和基于非线性的方法.基于滤波的方法主要有M ...
两个高斯分布乘积的理论推导
本文主要推导高斯分布(正态分布)的乘积,以便能更清楚的明白Kalman滤波的最后矫正公式. Kalman滤波主要分为两大步骤: 1.系统状态转移估计,2.系统测量矫正: 在第2步中的主要理论依据就是两 ...
Kalman滤波C语言和C++版本实现
写在前面 Kalman滤波的理论是比较有深度的,涉及到很多公式推导,尤其是用到了很多线性代数的知识. 这篇博客是博主基于工作内容实现的,所以主要偏向于应用,代码也是经过无数次验证了的,是比较稳定的,大 ...
理解Kalman滤波的使用
Kalman滤波简介 Kalman滤波是一种线性滤波与预测方法,原文为:A New Approach to Linear Filtering and Prediction Problems.文章推导很 ...
四旋翼姿态解算——梯度下降法理论推导
转载请注明出处:http://blog.csdn.net/hongbin_xu 或 http://hongbin96.com/ 文章链接:http://blog.csdn.net/hongbin_xu ...
时序分析：Kalman滤波（状态空间）
在现实生活中, 数据的出现大多数是以非平稳形式, 这就涉及到了动态数据所构成的时间序列的分解.关于时间序列的分解, PeterJ.Brochwell&RichardA.Davis在其著作< ...
霍普分叉matlab程序,基于MATLAB_GUI的Kalman滤波程序
第1章Kalman滤波理论 1.1 Kalman滤波理论的应用背景信号是传递和运载信息的时间或空间函数.信号有两类,即确定性信号和随即信号.确定性信号的变化规律是既定的,可以表示为一确定的时间函数或 ...
【C#】【引用加原创】C#实现kalman滤波
最近为了项目,同事让我帮他做一个硬件版的kalman滤波器,实现对设备的kalman滤波,以验证他的理论算法. 犹豫了好几天,用dsp吧,我的kalman滤波算法比较简单,有点大材小用.刚好手里有一块 ...

kalman滤波纯纯纯纯纯纯理论推导

kalman滤波纯纯纯纯纯纯理论推导相关推荐

最新文章

热门文章