卡尔曼滤波器入门

卡尔曼滤波器入门1第1页，共20页，2023年，2月20日，星期一数字滤波：通过一种算法排除可能的随机干扰，提高检测精度的一种手段2第2页，共20页，2023年，2月20日，星期一RudolfEmilKalman匈牙利数学家BS&MSatMITPhDatColumbia1960年发表的论文《ANewApproachtoLinearFilteringandPredictionProblems》（线性滤波与预测问题的新方法）3第3页，共20页，2023年，2月20日，星期一应用场合机器人导航、控制传感器数据融合雷达系统以及导弹追踪计算机图像处理头脸识别图像分割图像边缘检测4第4页，共20页，2023年，2月20日，星期一KF是根据上一状态的估计值和当前状态的观测值推出当前状态的估计值的滤波方法S(t)=f(S(t-1),O(t))它是用状态方程和递推方法进行估计的，因而卡尔曼滤波对信号的平稳性和时不变性不做要求维纳滤波：使用全部观测值保证平稳性5第5页，共20页，2023年，2月20日，星期一卡尔曼滤波器是一个 optimalrecursivedataprocessingalgorithm 最优化自回归数据处理算法最优（optimal）依赖于评价性能的判据。 Kalman滤波器充分利用如下信息估 a.系统和测量装置的动态特性； b.系统噪声、测量误差和动态模型的不确定性的统计描述； c.感兴趣变量的初始条件的相关信息。递归（recursive）是指kalman不需要保存先前的数据，当进行新的测量时也不需要对原来数据进行处理。6第6页，共20页，2023年，2月20日，星期一房间温度的当前感觉值房间温度计的当前读数五分钟以后房间温度的实际值问题描述已知条件希望得到都带有误差感觉值+测量值未来时刻的真实值？=7第7页，共20页，2023年，2月20日，星期一x(k+1)=Ax(k)+Bu(k)+w(k)z(k)=Hx(k)+v(k)状态方程stateequation测量方程measurementequation状态空间模型8第8页，共20页，2023年，2月20日，星期一9第9页，共20页，2023年，2月20日，星期一x(k)k时刻k+1时刻k-1时刻z(k+1)x(k+1|k+1)u(k)z(k)10第10页，共20页，2023年，2月20日，星期一x(k+1)=Ax(k)+Bu(k)+w(k)z(k)=Hx(k)+v(k)x(k+1)z(k+1)+x(k+1|k+1)思路：加权求和11第11页，共20页，2023年，2月20日，星期一x(k+1)=Ax(k)+Bu(k)+w(k)k时刻k+1k-1x(k+1|k)=Ax(k|k)+Bu(k)+w(k)①x(k+1|k+1)12第12页，共20页，2023年，2月20日，星期一P(k+1|k)=AP(k|k)A’+Q②x(k+1|k)对应的协方差也需要预测13第13页，共20页，2023年，2月20日，星期一k时刻k+1k-1x(k+1|k+1)至此，完成了预测阶段得到了两个量：下一时刻的状态预测x(k+1|k)描述x(k+1|k)优劣程度的协方差P(k+1|k)14第14页，共20页，2023年，2月20日，星期一z(k+1)=Hx(k+1|k)+v(k+1)K时刻k+1时刻k-1测量值z(k+1)x(k+1|k+1)x(k+1|k)P(k+1|k)15第15页，共20页，2023年，2月20日，星期一K时刻k+1时刻k-1x(k+1|k)P(k+1|k)y(k+1)=z(k+1)-Hx(k+1|k)k+1时刻，测量余量y(k+1)的表达式测量余量y(k+1)的协方差S=HP(k+1|k)H’+R测量余量y(k+1)16第16页，共20页，2023年，2月20日，星期一x(k+1|k+1)=x(k+1|k)+Ky(k+1)K(k+1)=P(k+1|k)H’/S③④x(k+1|k)y(k+1)加权x(k+1|k+1)17第17页，共20页，2023年，2月20日，星期一P(k+1|k+1)=(I-K(k+1)H）P(k+1|k)x(k+1|k+1)