扩展Kalman滤波(EKF)和无迹卡尔曼滤波 PPT

合集下载

卡尔曼滤波器 ppt课件

卡尔曼滤波器的应用
• 卡尔曼滤波器对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑使用了这种滤波器。
• 它的广泛应用已经超过30年，包括导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等,尤其是在自动或辅助导航系统。近年来更被应用于计算机视觉领域，例如人脸识别，运动物体跟踪等等。
卡尔曼滤波器的思想
• 基本思想：卡尔曼滤波器提供了一种有效的以最小均方误差来估算系统状态计算递归方法。若有一组强而合理的假设，给出系统的历史测量值，则可以建立最大化这些早前测量值的后验概率的系统状态模型。并且无需存储很长的早前测量历史，我们也可以最大化后验概率，即重复更新系统状态模型，并只为下一次更新保存模型。这样就大大地简化了这个方法的计算机实现。
• 最常用的是最小二乘估计，其他如风险准则的贝叶斯估计、最大似然估计、随机逼近等方法也都有应用。不管是维纳滤波还是卡尔曼滤波，这些方法都只适用于线性系统，而且需要对被估计过程有充分的知识。对于非线性系统或对动态系统特性不完全了解的复杂估计问题，还需要深入研究。工程上可用一些近似计算方法来处理，常见的有基于局部线性化思想的广义卡尔曼滤波器、贝叶斯或极大后验估值器和可以根据滤波过程的历史知识自动修改参数的自适应滤波或预报技术等
卡尔曼滤波器
1
卡尔曼滤波器
精品资料
你怎么称呼老师？如果老师最后没有总结一节课的重点的难点，你
是否会认为老师的教学方法需要改进？你所经历的课堂，是讲座式还是讨论式？教师的教鞭 “不怕太阳晒，也不怕那风雨狂，只怕先生骂我
笨，没有学问无颜见爹娘 ……” “太阳当空照，花儿对我笑，小鸟说早早早……”
• 卡尔曼滤波的实质是由量测值重构系统的状态向量。它以“预测—实测—修正” 的顺序递推，根据系统的量测值来消除随机干扰，再现系统的状态，或根据系统的量测值从被污染的系统中恢复系统的本来面目。

扩展Kalman滤波(EKF)和无迹卡尔曼滤波(ukf)

UKF算法的核心是UT变换，UT是一种计算非线性变换中的随机变量的统计特征的新方法，是UKF的基础。
三、无迹卡尔曼滤波算法(UKF)
假设n维随机向量x : N(x, Px) ,x通过非线性函数y=f(x) 变换后得到n维的随机变量y。通过UT变换可以以较高的精度和较低的计算复杂度求得y的均值 y 和方差 Px 。UT的具体过程可描述如下:
三、无迹卡尔曼滤波算法(UKF)
UKF是用确定的采样来近似状态的后验PDF,可以有效解决由系统非线性的加剧而引起的滤波发散问题，但UKF仍是用高斯分布来逼近系统状态的后验概率密度，所以在系统状态的后验概率密度是非高斯的情况下，滤波结果将有极大的误差。
三、无迹卡尔曼滤波算法(UKF)
： Matlab程序

dax = 1.5; day = 1.5; % 系统噪声

X = zeros(len,4); X(1,:) = [0, 50, 500, 0]; % 状态模拟的初值

for k=2:len
x = X(k-1,1); vx = X(k-1,2); y = X(k-1,3); vy = X(k-1,4);

F(3,4) = 1;

F(4,4) = 2*ky*vy;

2) = 1;
二、扩展Kalman滤波(EKF)算法
function H = JacobianH(X) % 量测雅可比函数

x = X(1); y = X(3);

H = zeros(2,4);

r = sqrt(x^2+y^2);
二、扩展Kalman滤波(EKF)算法
EKF算法是一种近似方法，它将非线性模型在状态估计值附近作泰勒级数展开，并在一阶截断，用得到的一阶近似项作为原状态方程和测量方程近似表达形式，从而实现线性化同时假定线性化后的状态依然服从高斯分布，然后对线性化后的系统采用标准卡尔曼滤波获得状态估计。采用局部线性化技术，能得到问题局部最优解，但它能否收敛于全局最优解，取决于函数的非线性强度以及展开点的选择。

卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波以及粒子滤波原理

卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波以及粒子滤波原理所有滤波问题其实都是求感兴趣的状态的后验概率分布，只是由于针对特定条件的不同，可通过求解递推贝叶斯公式获得后验概率的解析解（KF、EKF、UKF），也可通过大数统计平均求期望的方法来获得后验概率（PF）。

1 KF、EKF、UKF1.1 定义KF、EKF、UKF 都是一个隐马尔科夫模型与贝叶斯定理的联合实现。

是通过观测信息及状态转移及观测模型对状态进行光滑、滤波及预测的方法。

而KF、EKF及UKF的滤波问题都可以通过贝叶斯估计状态信息的后验概率分布来求解。

Kalman在线性高斯的假设下，可以直接获得后验概率的解析解；EKF是非线性高斯模型，通过泰勒分解将非线性问题转化为线性问题，然后套用KF的方法求解，缺陷是线性化引入了线性误差且雅克比、海塞矩阵计算量大；而UKF也是非线性高斯模型，通过用有限的参数来近似随机量的统计特性，用统计的方法计算递推贝叶斯中各个积分项，从而获得了后验概率的均值和方差。

1.2 原理KF、EKF、UKF滤波问题是一个隐马尔科夫模型与贝叶斯定理的联合实现。

一般的状态模型可分为状态转移方程和观测方程，而状态一般都是无法直接观测到的，所以时隐马尔科夫模型。

然后，它将上一时刻获得的状态信息的后验分布作为新的先验分布，利用贝叶斯定理，建立一个贝叶斯递推过程，从而得到了贝叶斯递推公式，像常用的卡尔曼滤波、扩展卡尔曼滤波、不敏卡尔曼滤波以及粒子滤波都是通过不同模型假设来近似最优贝叶斯滤波得到的。

这也是滤波问题的基本思路。

所有贝叶斯估计问题的目的都是求解感兴趣参数的后验概率密度。

并且后验概率的求解是通过递推计算目标状态后验概率密度的方法获得的。

在贝叶斯框架下，通过状态参数的先验概率密度和观测似然函数来求解估计问题；在目标跟踪背景下（隐马尔科夫模型），目标动态方差决定状态转移概率，观测方程决定释然函数。

一般化的整个计算过程可以分为3步：01. 一步状态预测：通过状态转移概率及上一时刻的后验概率算出一步预测概率分布。

卡尔曼滤波方法资料课件

采用最小均方误差准则，通过最小化估计误差的平方和实现状态估计。
线性最小方差估计方法的优点
适用于线性系统状态估计，计算量较小，易于实现。
线性最小方差估计方法的缺点
对非线性系统效果不佳，需要先验知识或模型参数。
04
卡尔曼滤波方法的实现和应用案例
卡尔曼滤波方法的软件实现
软件平台
可以使用Python、C、Matlab等编程语言实现卡尔曼滤波算法。
卡尔曼滤波方法在控制系统中的应用案例
应用场景
卡尔曼滤波方法在控制系统中主要用于估计系统的状态变量。
案例分析
通过实际控制系统的数据和实验，验证卡尔曼滤波方法在控制系统中的可行性和稳定性。
卡尔曼滤波方法在雷达系统中的应用案例
应用场景
卡尔曼滤波方法在雷达系统中主要用于目标跟踪和运动参数估计。
VS
案例分析
卡尔曼滤波方法的基本概念和原理
基本概念
卡尔曼滤波方法是一种递归估计方法，通过建立状态方程和观测方程，对系统状态进行最优估计。
原理
卡尔曼滤波方法基于最小均方误差准则，通过不断更新估计值来逼近真实值，具有计算量小、实时性强的优点。
卡尔曼滤波方法的应用领域
机器人
用于机器人的定位、路径规划、避障等。
描述系统状态和观测之间的关系。
定义初始状态和误差协方差
02
确定系统初始状态和误差协方差的估计值，为后续的滤波过程
提供初始条件。
选择合适的模型参数
03
根据实际情况选择合适的模型参数，如系统动态参数、观测参
数等，以更好地描述系统特性。
预测步骤
01
根据上一时刻的状态和误差协方差，预测当前时刻的系统状态和误差协方差。

卡尔曼滤波介绍ppt课件(共29张PPT)

卡尔曼滤波是一种利用目标动态信息去除噪声影响，得到目标位置良好估计的方法。它适用于雷达跟踪等场景，其中目标位置、速度、加速度的测量值常含有噪声。通过贝叶斯理论推导，卡尔曼滤波能够实现对当前、未来或过去位置的估计，分别对应滤波、预测和插值或平滑操作。典型实例包括从有限且包含噪声的观察序列中预测物体位置坐标及速度。此外，文档还探讨了扩展卡尔曼滤波（EKF）的推导过程，并展示了其在无人车定位等实际应用中的价值。紧组，突显了卡尔曼滤波在处理有色噪声及提高导航系统性能方面的重要性。

卡尔曼滤波.ppt

头脸识别图像分割图像边缘检测
Temperature Problem - Ideal World

假设当前室内温度仅跟上一时刻有关温度计观测（摄氏－〉华氏）根据连续的观测值来推算实际温度变化
Temperature Problem - Real World

假设当前室内温度仅跟上一时刻有关
为

先验误差和后验误差odel - Algorithm

递推公式

如果没有误差，可以认为则包含全部误差的信息，称为新息 (innovation) K为修正矩阵，或称混合因子 (Blend factor)

Blend factor Matrix

修正矩阵的形式有多种，其中一种为：

R->0 => K = 1/H
Discrete KF
Flow Chart
任意给定初值均可，但P!=0
Experiment
目标：
用KF估计一个常数（电压）
约束：
数据本身有误差（电压不稳）
观测有误差（电压表不准）
Analysis – Matrix Assignment

通过一种算法排除可能的随机干扰提高检测精度的一种手段线性系统?线性系统fabfafb?数学方法处理?噪声信号输入尽可能少噪声输出usefor?机器人导航控制?传感器数据融合?雷达系统以及导弹追踪?计算机图像处理?头脸识别?图像分割?图像边缘检测temperatureproblemidealworld?假设当前室内温度仅跟上一时刻有关?温度计观测摄氏华氏?根据连续的观测值来推算实际温度变化temperatureproblemrealworld?假设当前室内温度仅跟上一时刻有关?但变化中可能有噪声温度计观测摄氏?温度计观测摄氏华氏华氏?读数会有误差?两种噪声相互无关?根据连续的观测值来推算实际温度变化kalmanfilteringfirstsight?kf是根据上一状态的估计值和当前状态的观测值推出当前状态的估计值的滤波方法?stfst1ot?它是用状态方程和递推方法进行估计的因而卡尔曼滤波对信号的平稳性和时不变性不做要求?维纳滤波

扩展Kalman滤波(EKF)和无迹卡尔曼滤波(ukf).共31页

谢谢！
51、天下之事常成于困约，而败于奢靡。——陆游 52、生命不等于是呼吸，生命是活动。——卢梭
53、伟大的事业，需要决心，能力，组织和责任感。 ——易卜生 54、唯书籍不朽。——乔特
55、为中华之崛起而读书。 ——周恩来
扩展Kalman滤波(EKF)和无迹卡尔曼滤波(ukf).
11、用道德的示范来造就一个人，显然比用法律来约束他更有价值。—— 希腊
12、法律是无私的，对谁都一视同仁。在每件事制不了好的自由，因为好人不会去做法律不允许的事情。——弗劳德
14、法律是为了保护无辜而制定的。——爱略特 15、像房子一样，法律和法律都是相互依存的。——伯克

卡尔曼滤波方法PPT课件

17
第17页/共28页
联邦滤波器算法
• 信息分配
在进入下一次递推之前，需将主滤波器中的信息（状态、方差）在各子滤波器中按如下规则进行分配：
N
Xˆ i Xˆ g ,
Pii
P 1
ig
,
Q1
Qi1 Qm1
i 1
其中，Qi m1Q , i , i 1,, N, m 为信息分配系数，m 为
主滤波器的信息分配系数，满足守恒原则
方差估值 Pk k [I Kk Hk ]Pk k1
6
第6页/共28页
3.5 卡尔曼滤波的结构图
上述递推公式，称为卡尔曼滤波器。实际上，卡尔曼滤波器也是一个系统，其结构框图如下：
Zk + -
+
Kk
+
Z k|k 1
当前估计值
Xˆ k
延时一步
Hk
k ,k 1
一步预测
上一步估计值
Xˆ k|k 1
第27页/共28页
感谢您的观看！
28
第28页/共28页
Yi f ( i )
24
第24页/共28页
Unscented卡尔曼滤波(续) 变换样本点Yi 即可近似表示 y 的分布。下面利用 Yi 来计算 y 的均值和方差。
3. 计算 y 的均值和方差
p
y Wi(m)Yi
i0
p
Py Wi(c) (Yi y)(Yi y)T i0
其中，
Wi(m)
Wi(c)
得预测测量估计偏差： Z~k|k1 Zk Zˆk|k1 Zk Hk Xˆ k|k1
利用此偏差修正预测估计：
Xˆ k|k Xˆ k k1 Kk [Zk Hk Xˆ k k1]

《卡尔曼滤波》课件

3
无迹卡尔曼滤波线性系统的估计。
卡尔曼滤波的应用案例
飞行器姿态估计
卡尔曼滤波在航空领域中被广泛应用于飞行器姿态估计，用于提高飞行器的稳定性和导航准确性。
目标跟踪
卡尔曼滤波可用于跟踪移动目标的位置和速度，常见于机器人导航和视频监控等领域。
3 卡尔曼滤波的应用领
域
卡尔曼滤波被广泛应用于航空航天、机器人、金融等领域，用于提高系统的状态估计精度。
卡尔曼滤波的数学模型
状态空间模型
卡尔曼滤波使用状态空间模型表示系统的状态和观测值之间的关系，包括状态方程和测量方程。
测量方程
测量方程描述观测值与系统状态之间的关系，用于将观测值纳入到状态估计中。
了解更多关于卡尔曼滤波的内容和应用，推荐文献、学术论文和在线课程等资源。
《卡尔曼滤波》PPT课件
卡尔曼滤波是一种优秀的状态估计方法，被广泛用于目标跟踪、姿态估计和股票预测等领域。
介绍卡尔曼滤波
1 什么是卡尔曼滤波？
卡尔曼滤波是一种递归状态估计算法，用于通过系统模型和测量信息估计系统状态。
2 卡尔曼滤波的基本原
理
卡尔曼滤波基于贝叶斯估计理论，通过最小化估计误差的均方差来优化状态估计。
股票预测
卡尔曼滤波可以应用于股票市场，通过对历史数据进行分析和预测，提供股票价格的预测和趋势分析。
卡尔曼滤波的优化算法
粒子滤波
粒子滤波是一种基于蒙特卡洛方法的状态估计算法，适用于非线性和非高斯系统，提供更广泛的估计能力。
自适应滤波
自适应滤波是一种根据系统的特点自动调整滤波参数的方法，提供更好的适应性和鲁棒性。
非线性滤波
非线性滤波是对卡尔曼滤波算法的改进，用于处理非线性系统和测量模型，提供更准确的状态估计。

扩展卡尔曼滤波和无迹卡尔曼滤波

二、扩展Kalman滤波(EKF)算法
假定定位跟踪问题的非线性状态方程和测量方程如下:
X k1 f ( X k ) Wk ...............(1)
Yk h( X k ) Vk ...................(.2)

在最近一次状态估计的时刻，对以上两式进
行线性化处理，首先构造如下2个矩阵:
%---------------------------------------
function UKFmain
%------------------清屏----------------
close all;clear all;
clc; tic;
global Qf n;
%定义全局变量
%------------------初始化--------------

dax = 1.5; day = 1.5; % 系统噪声

X = zeros(len,4); X(1,:) = [0, 50, 500, 0]; % 状态模拟的初值

for k=2:len
x = X(k-1,1); vx = X(k-1,2); y = X(k-1,3); vy = X(k-1,4);
2n
Py wic (Yi y)(Yi y)T i0
................(9)
2n
Pxy wic (Yi x)(Yi y)T i0
..............(10)
由于x的均值和方差都精确到二阶，计算得到y 的均值和方差也精确到二阶，比线性化模型精度更高。在卡尔曼框架内应用UT技术就得到了UKF算法。

r = sqrt(X(k,1)^2+X(k,3)^2) + dr*randn(1,1);

扩展卡尔曼滤波原理及其应用PPT课件

到现在为止，我们已经得到了k 状态下最优的估算值X(k|k)。但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的covariance：
P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)
其中I 为1的矩阵，对于单模型单测量，I=1。当系统进入 k+1状态时，P(k|k)就是式子(2) 的P(k-1|k-1)。这样，算法就可以自回归的运算下去。
F f ( xk 1) x
Hk
h( xk /k 1 ) x
xk1 xk vx _ k t yk1 yk vy _ k t
vx _ k1 vx _ k (ax _ k cos ay sin ) t vy _ k1 vy _ k (ax _ k sin ay cos ) t k1 k t
现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结合预测值和测量值，我们可以得到现在状态(k) 的最优化估算值X(k|k)：
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
其中Kg为卡尔曼增益(Kalman Gain)： Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)
在进入k+1时刻之前，我们还要算出k 时刻那个最优值（24.56度）的偏差。算法如下：((1-Kg)*5^2) ^0.5=2.35。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入k+1时刻以后k 时刻估算出的最优温度值的偏差（对应于上面的3）。
1. 首先我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k ，根据系统的模型，可以基于系统的上一状态而预测出现在状态： X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)

卡尔曼滤波教学课件PPT

5.卡尔曼滤波控制系统结构图
由于系统的状态x是不确定的，卡尔曼滤波器的任务就是在有随机干扰w和噪声v的情 ˆ ，它在况下给出系统状态x的最优估算值 x 统计意义下最接近状态的真值x，从而实现最优控制u( x ˆ )的目的。
状态方程：X(k)=AX(k-1)+BU(k)+W(k) 输出方程：y(k)=CX(k)+Z(k) 系统测量值：Z(k)=HX(k)+V(k) 在上述方程中，X(k)是k时刻的系统状态， U(k)是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，它们为矩阵。 Z(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。W(k)和 V(k)分别表示过程噪声和测量噪声。它们被假设成高斯白噪声，它们的协方差分别是Q， R。
6.2
更新阶段
新息或测量余量：y(k)=Z(k)-H X(k|k-1) 新息协方差:S(k)=H P(k|k-1) H’ +R 卡尔曼增益(Kalman Gain): Kg(k)= P(k|k1) H’ / (H P(k|k-1) H’ + R) …… (3) 状态估计更新:收集现在状态的测量值，结合预测值和测量值，可以得到现在状态的最优化估算值。
6.卡尔曼滤波过程
卡尔曼滤波包括两个阶段：预测和更新。在预测阶段，滤波器应用上一状态的估计做出对当前状态的估计。在更新阶段，滤波器利用在当前状态的观测值优化预测阶段的预测值，以获的一个更精确的当前状态的估计。
6.1预测阶段
状态估计：根据系统的模型，可以基于系统的上一状态而预测出现在的状态。 X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1) 式(1)中，A是作用在前一状态的状态转移模型(状态转移矩阵),B是作用在控制向量上的控制输入模型(输入输出矩阵), X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果，U(k) 为现在状态的控制量，如果没有控制量，它可以为0 。

扩展Kalman滤波(EKF)和无迹卡尔曼滤波(ukf)分析

二、扩展Kalman滤波(EKF)算法

vy = vy + (ky*vy^2-g+day*randn(1))*Ts; X(k,:) = [x, vx, y, vy]; end figure(1), hold off, plot(X(:,1),X(:,3),'-b'), grid on % figure(2), plot(X(:,2:2:4)) % 构造量测量 mrad = 0.001; dr = 10; dafa = 10*mrad; % 量测噪声 for k=1:len r = sqrt(X(k,1)^2+X(k,3)^2) + dr*randn(1,1); a = atan(X(k,1)/X(k,3)) + dafa*randn(1,1); Z(k,:) = [r, a]; end
成线性问题。对于非线性问题线性化常用的两大途径：
（1）将非线性环节线性化，对高阶项采用忽略或逼近措施；（EKF）
（2）用采样方法近似非线性分布. ( UKF)
二、扩展Kalman滤波(EKF)算法

EKF算法是一种近似方法，它将非线性模型在状态估计值附近作泰勒级数展开，并在一阶截断，用得到的一阶近似项作为原状态方程和测量方程近似表达形式，从而实现线性化同时假定线性化后的状态依然服从高斯分布，然后对线性化后的系统采用标准卡尔曼滤波获得状态估计。采用局部线性化技术，能得到问题局部最优解，但它能否收敛于全局最优解，取决于函数的非线性强度以及展开点的选择。
二、扩展Kalman滤波(EKF)算法

figure(1), plot(X_est(:,1),X_est(:,3), '+r') xlabel('X'); ylabel('Y'); title('ekf simulation'); legend('real', 'measurement', 'ekf estimated'); %%%%%%%%%%%%%%%%%%%%子程序%%%%%%%%%%%%%%%%%%% function F = JacobianF(X, kx, ky, g) % 系统状态雅可比函数 = X(2); vy = X(4); F = zeros(4,4); F(1, F(2,2) = -2*kx*vx; F(3,4) = 1; F(4,4) = 2*ky*vy; 2) = 1;

卡尔曼滤波算法(含详细推导)PPT

v1(n)G (n)v2(n)..........3 ...).0 ..19..(
3、kalman滤波算法
求式(3)所示状态向量的一步预测误差向量的相关矩阵，容易证明：
K(n1,n)E{e(n1,n)e]H(n1,n)} [F(n1,n)G (n)C (n)K ](n,n1)F [(n1,n) G (n)C (n)H ]Q 1(n)G (n)Q 2(n)G H(n)........3 ...).1 .(.
n
(n )(n 1y(1 ),y .(n .). ),
1
W 1 (k)(k)
式中W1(k)表示与一步预测项对应的权矩k 阵 1 ，且k为离散时间。
现在的问题是如何确定这个权矩阵？
（1）、状态向量的一布预测
根据正交性原理，最优预测的估计误差
e(1 nn, )x(n1)x1(n1)
12
3、kalman滤波算法
C (n )K (n ,n 1 )C H (n ) Q 2(n ).................1.).(6..
式中Q2(n)是观测噪声v2(n)的相关矩阵，而
K (n ,n 1 ) E { e (n ,n 1 )e H (n ,n 1 )}................1 ..) ....( 7 ..
这里使用了状态向量与观测噪声不相关的事实。进一步地，由正交原理引
理知，在最小均方误差准则下求得的一步预测估 x 1 ( n )与预测误差e(n,n-1)彼
此正交，即
E{x1(n)eH(N,N1)}0
17
3、kalman滤波算法
因此，由式(26)及式(27)易得：
E {x(n1)H(n)} F(n1,n)E {x[(n)e(n,n1)e]H(n,n1)C }H(n)

扩展Kalman滤波(EKF)和无迹卡尔曼滤波(ukf) ppt课件

fX = [x1; vx1; y1; vy1];
PPT课件
二、扩展Kalman滤波(EKF)算法
11
function hfX = hhh(fX, Ts) % 量测非线性函数
x = fX(1); y = fX(3);
r = sqrt(x^2+y^2);
a = atan(x/y);
hfX = [r; a];
n=4; T=3;
Qf=[T^2/2 0;0 T;T^2/2 0;0 T];
%--------------------------------------
stater=stater0;state=state0; xc=state;
function [Xk, Pk, Kk] = ekf(Phikk_1, Qk, fXk_1, Pk_1, Hk, Rk, Zk_hfX) % ekf 滤波函数
Pkk_1 = Phikk_1*Pk_1*Phikk_1' + Qk;
Pxz = Pkk_1*Hk'; Pzz = Hk*Pxz + Rk;
%------------------初始化-------------stater0=[220; 1;55;-0.5]; %标准系统初值 state0=[200;1.3;50;-0.3]; %测量状态初值 %--------系统滤波初始化
p=[0.005 0 0 0;0 0.005 0 0; 0 0 0.005 0;0 0 0 0.005]; %状态误差协方差初值
r = sqrt(X(k,1)^2+X(k,3)^2) + dr*randn(1,1);
a = atan(X(k,1)/X(k,3)) + dafa*randn(1,1);