超详细讲解无迹卡尔曼(UKF)滤波(个人整理结合代码分析) |
您所在的位置:网站首页 › 非线性滤波方法的比较与分析 › 超详细讲解无迹卡尔曼(UKF)滤波(个人整理结合代码分析) |
目录 1.用来做什么? 2.线性卡尔曼滤波 3.扩展卡尔曼滤波 4.无迹卡尔曼滤波 1.用来做什么?——针对系统的不确定性:1.不存在完美的数学模型 2.系统的扰动不可控、也很难建模 3.测量传感器存在误差 例1:通过系统的状态方程得出的电流值i1,和传感器测得的电流值i2,由于不确定性的存在,两个值都不准确,所以i1和i2通过卡尔曼滤波算法算出其最接近真实值的值。 例2:如小红同学说今天老师穿的是红色的衣服(根据以往经验,每周四老师都穿红衣服,小红得出的结论),小白说老师今天穿的白色的衣服(看到一个像老师的人穿的白色的衣服,小白得出老师穿白色衣服的结论),通过卡尔曼滤波推算谁说话的权重更大,则推断出更应该相信谁的话。 2.线性卡尔曼滤波系统真实方程为(状态方程的基础上存在噪声): 先验估计(他叫这名): ——(注意:噪声满足 后验估计(所要求的值): 因为: 其目标为P的期望为0或方差最小(使后验估计与真实值误差最小),通过推导可以得到卡尔曼增益: 同时可以得到
卡尔曼滤波主要用于分析线性系统。 扩展卡尔曼滤波主要用于分析非线性系统。(正态分布的随机变量通过非线性系统后就不再是正态的了) 。 扩展卡尔曼滤波——主要是通过泰勒级数一阶展开,将非线性系统线性化,之后的求取与线性化卡尔曼滤波一致。 非线性系统:
泰勒级数一阶展开式:
对非线性系统线性化。系统有误差,无法在真实点线性化,则
令
先验估计: 后验估计: 标红部分为扩展和线性卡尔曼滤波的不同之处。 4.无迹卡尔曼滤波由于扩展卡尔曼滤波可能存在线性化误差,且一般情况下雅克比矩阵不易实现,增加了算法的计算复杂度。 无迹卡尔曼滤波不采用泰勒展开实现非线性系统线性化,而是采用无迹变换(Unscented Transform,UT)来处理均值和协方差的非线性传递问题。(UKF算法是对非线性函数的概率密度分布进行近似,用一系列确定样本来逼近状态的后验概率密度,而不是对非线性函数进行近似,不需要对雅克比矩阵进行求导。) 无迹变换:——(1)原状态分布中按某一规则选取一些采样点(其均值和方差等于原状态分布的均值和方差) —— (2)将点带入非线性方程中(求取变换后的均值和协方差) 以对称分布采样的UT变换为例。设一个非线性变换 (1)计算2n+1个sigma点,即采样点
设:
则:
合并起来就是9个sigma点: sig1 sig2 sig3 sig4 sig5 sig6 sig7 sig8 sig9 (2)计算采样点相应的权值 将9个sigma点带入非线性方程得到新的sigma点:
假设得到 sig1 sig2 sig3 sig4 sig5 sig6 sig7 sig8 sig9 定义权值 可求得下一个点的先验值(经过UT变换后得到的先验值)
以上是无迹变换算先验值的整个过程。 无迹卡尔曼滤波算法:非线性系统: 步骤: ————1、经过UT变换求得sigma采样点及其权值 ————2、计算2n+1个sigma点集的一步预测 ————3、系统状态量的一步预测(相当于KF/EKF的先验值)
————4、再次使用UT变换,产生新的sigma点集 ————5、新的sigma点集带入观测方程,得到预测的观测量
————6、通过加权求得观测量新的均值及协方差
————7、 计算卡尔曼增益 ————8、系统的状态更新和协方差更新 图1. 卡尔曼滤波流程图 实际应用(仿真分析):系统方程:
真实状态信息为: UKF滤波算法得到的目标状态为: 定义均方根误差(RMSE): T=1;N=60/T;X=zeros(4,N); ——定义X为4行60列的数列X(:,1)=[-100,2,200,20]; ——第一个点Z=zeros(1,N); ——定义Z为1行60列的数列delta_w=1e-3;Q=delta_w*diag([0.5,1]); ——状态噪声G=[T^2/2,0;T,0;0,T^2/2;0,T];R=5; ——观测噪声F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];x0=200;y0=300; ——可以随便给定(Z方程里的值) Xstation=[x0,y0];v=sqrtm(R)*randn(1,N);for t=2:N X(:,t)=F*X(:,t-1)+G*sqrtm(Q)*randn(2,1);endfor t=1:N Z(t)=Dist(X(:,t),Xstation)+v(t);end ——真实状态值和观测值
L=4;alpha=1;kalpha=0;belta=2;ramda=3-L;for j=1:2*L+1 Wm(j)=1/(2*(L+ramda)); Wc(j)=1/(2*(L+ramda));endWm(1)=ramda/(L+ramda); ——求第一次方差的权值Wc(1)=ramda/(L+ramda)+1-alpha^2+belta; ——求第一次均值的权值 Wm Wc Xukf=zeros(4,N);Xukf(:,1)=X(:,1);P0=eye(4); for t=2:N ——(注:下面给出的数据是第一次循环) xestimate=Xukf(:,t-1);
xgamaP1的2列与xgamaP2的2列对称 xgamaP1的3列与xgamaP2的3列对称 xgamaP1的4列与xgamaP2的4列对称
Xsigmapre=F*Xsigma; ——9个sigma点带入非线性函数得到新的9个sigma点 Xpred=zeros(4,1); for k=1:2*L+1 Xpred=Xpred+Wm(k)*Xsigmapre(:,k); ——新的状态值(先验值)4行1列 end Ppred=zeros(4,4); for k=1:2*L+1 Ppred=Ppred+Wc(k)*(Xsigmapre(:,k)-Xpred)*(Xsigmapre(:,k)-Xpred)'; end Ppred=Ppred+G*Q*G'; ——新的协方差 chor=(chol((L+ramda)*Ppred))'; for k=1:L XaugsigmaP1(:,k)=Xpred+chor(:,k); XaugsigmaP2(:,k)=Xpred-chor(:,k); end Xaugsigma=[Xpred XaugsigmaP1 XaugsigmaP2]; ——先验值经UT变化得到新的9个sigma点 (用来算Z值)
Pzz=0; for k=1:2*L+1 Pzz=Pzz+Wc(k)*(Zsigmapre(1,k)-Zpred)*(Zsigmapre(1,k)-Zpred)'; ——用来求卡尔曼增益 end Pzz=Pzz+R;
for i=1:N Err_KalmanFilter(i)=Dist(X(:,i),Xukf(:,i));endfigurehold on;box on;plot(X(1,:),X(3,:),'-k.');plot(Xukf(1,:),Xukf(3,:),'-r+');legend('真实轨迹','UKF轨迹')figurehold on;box on;plot(Err_KalmanFilter,'-ks','MarkerFace','r') 调用函数1: function d=Dist(X1,X2)if length(X2) |
今日新闻 |
点击排行 |
|
推荐新闻 |
图片新闻 |
|
专题文章 |
CopyRight 2018-2019 实验室设备网 版权所有 win10的实时保护怎么永久关闭 |