KF和EKF和UKF介绍matlab源码程序 - matlab通信信号

下载频道> 资源分类> matlab源码> 通信信号> KF和EKF和UKF介绍matlab源码程序

标题：KF和EKF和UKF介绍matlab源码程序
分享到：

所属分类: 通信信号资源类型：程序源码文件大小: 240.66 KB 上传时间: 2019-06-17 22:36:34 下载次数: 56 资源积分：1分提供者: zhangsan456 code

内容：
KF和EKF和UKF介绍matlab源码程序

% EKF UKF PF 的三个算法

clear;

% tic;

x = 0.1; % 初始状态

x_estimate = 1;%状态的估计

e_x_estimate = x_estimate; %EKF的初始估计

u_x_estimate = x_estimate; %UKF的初始估计

p_x_estimate = x_estimate; %PF的初始估计

Q = 10;%input('请输入过程噪声方差Q的值: '); % 过程状态协方差

R = 1;%input('请输入测量噪声方差R的值: '); % 测量噪声协方差

P =5;%初始估计方差

e_P = P; %EKF方差

u_P = P;%UKF方差

pf_P = P;%PF方差

tf = 50; % 模拟长度

x_array = [x];%真实值数组

e_x_estimate_array = [e_x_estimate];%EKF最优估计值数组

u_x_estimate_array = [u_x_estimate];%UKF最优估计值数组

p_x_estimate_array = [p_x_estimate];%PF最优估计值数组

u_k = 1; %微调参数

u_symmetry_number = 4; % 对称的点的个数

u_total_number = 2 * u_symmetry_number + 1; %总的采样点的个数

linear = 0.5;

N = 500; %粒子滤波的粒子数

close all;

%粒子滤波初始 N 个粒子

for i = 1 : N

p_xpart(i) = p_x_estimate + sqrt(pf_P) * randn;

end

for k = 1 : tf

% 模拟系统

x = linear * x + (25 * x / (1 + x^2)) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn; %状态值

y = (x^2 / 20) + sqrt(R) * randn; %观测值

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%扩展卡尔曼滤波器%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%进行估计第一阶段的估计

e_x_estimate_1 = linear * e_x_estimate + 25 * e_x_estimate /(1+e_x_estimate^2) + 8 * cos(1.2*(k-1));

e_y_estimate = (e_x_estimate_1)^2/20; %这是根据k=1时估计值为1得到的观测值；只是这个由我估计得到的第24行的y也是观测值不过是由加了噪声的真实值得到的

%相关矩阵

e_A = linear + 25 * (1-e_x_estimate^2)/((1+e_x_estimate^2)^2);%传递矩阵

e_H = e_x_estimate_1/10; %观测矩阵

%估计的误差

e_p_estimate = e_A * e_P * e_A' + Q;

%扩展卡尔曼增益

e_K = e_p_estimate * e_H'/(e_H * e_p_estimate * e_H' + R);

%进行估计值的更新第二阶段

e_x_estimate_2 = e_x_estimate_1 + e_K * (y - e_y_estimate);

%更新后的估计值的误差

e_p_estimate_update = e_p_estimate - e_K * e_H * e_p_estimate;

%进入下一次迭代的参数变化

e_P = e_p_estimate_update;

e_x_estimate = e_x_estimate_2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%粒子滤波器%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i = 1 : N

p_xpartminus(i) = 0.5 * p_xpart(i) + 25 * p_xpart(i) / (1 + p_xpart(i)^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn; %这个式子比下面一行的效果好

% xpartminus(i) = 0.5 * xpart(i) + 25 * xpart(i) / (1 + xpart(i)^2) + 8 * cos(1.2*(k-1));

p_ypart = p_xpartminus(i)^2 / 20; %预测值

p_vhat = y - p_ypart;% 观测和预测的差

p_q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-p_vhat^2 / 2 / R); %各个粒子的权值

end

% 平均每一个估计的可能性

p_qsum = sum(p_q);

for i = 1 : N

p_q(i) = p_q(i) / p_qsum;%各个粒子进行权值归一化

end

% 重采样权重大的粒子多采点，权重小的粒子少采点, 相当于每一次都进行重采样；

for i = 1 : N

p_u = rand;

p_qtempsum = 0;

for j = 1 : N

p_qtempsum = p_qtempsum + p_q(j);

if p_qtempsum >= p_u

p_xpart(i) = p_xpartminus(j); %在这里 xpart(i) 实现循环赋值；终于找到了这里！！！

break;

end

p_x_estimate = mean(p_xpart);

% p_x_estimate = 0;

% for i = 1 : N

% p_x_estimate =p_x_estimate + p_q(i)*p_xpart(i);

% end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%不敏卡尔曼滤波器%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%采样点的选取存在x(i)

u_x_par = u_x_estimate;

for i = 2 : (u_symmetry_number+1)

u_x_par(i,:) = u_x_estimate + sqrt((u_symmetry_number+u_k) * u_P);

end

for i = (u_symmetry_number+2) : u_total_number

u_x_par(i,:) = u_x_estimate - sqrt((u_symmetry_number+u_k) * u_P);

end

%计算权值

u_w_1 = u_k/(u_symmetry_number+u_k);

u_w_N1 = 1/(2 * (u_symmetry_number+u_k));

%把这些粒子通过传递方程得到下一个状态

for i = 1: u_total_number

u_x_par(i) = 0.5 * u_x_par(i) + 25 * u_x_par(i)/(1+u_x_par(i)^2) + 8 * cos(1.2*(k-1));

end

%传递后的均值和方差

u_x_next = u_w_1 * u_x_par(1);

for i = 2 : u_total_number

u_x_next = u_x_next + u_w_N1 * u_x_par(i);

end

u_p_next = Q + u_w_1 * (u_x_par(1)-u_x_next) * (u_x_par(1)-u_x_next)';

for i = 2 : u_total_number

u_p_next = u_p_next + u_w_N1 * (u_x_par(i)-u_x_next) * (u_x_par(i)-u_x_next)';

end

% %对传递后的均值和方差进行采样产生粒子存在y(i)

% u_y_2obser(1) = u_x_next;

% for i = 2 : (u_symmetry_number+1)

% u_y_2obser(i,:) = u_x_next + sqrt((u_symmetry_number+k) * u_p_next);

% end

% for i = (u_symmetry_number + 2) : u_total_number

% u_y_2obser(i,:) = u_x_next - sqrt((u_symmetry_number+u_k) * u_p_next);

% end

%另外存在y_2obser(i) 中；

for i = 1 :u_total_number

u_y_2obser(i,:) = u_x_par(i);

end

%通过观测方程得到一系列的粒子

for i = 1: u_total_number

u_y_2obser(i) = u_y_2obser(i)^2/20;

end

%通过观测方程后的均值 y_obse

u_y_obse = u_w_1 * u_y_2obser(1);

for i = 2 : u_total_number

u_y_obse = u_y_obse + u_w_N1 * u_y_2obser(i);

end

%Pzz测量方差矩阵

u_pzz = R + u_w_1 * (u_y_2obser(1)-u_y_obse)*(u_y_2obser(1)-u_y_obse)';

for i = 2 : u_total_number

u_pzz = u_pzz + u_w_N1 * (u_y_2obser(i) - u_y_obse)*(u_y_2obser(i) - u_y_obse)';

end

%Pxz状态向量与测量值的协方差矩阵

u_pxz = u_w_1 * (u_x_par(1) - u_x_next)* (u_y_2obser(1)-u_y_obse)';

for i = 2 : u_total_number

u_pxz = u_pxz + u_w_N1 * (u_x_par(i) - u_x_next) * (u_y_2obser(i)- u_y_obse)';

end

%卡尔曼增益

u_K = u_pxz/u_pzz;

%估计量的更新

u_x_next_optimal = u_x_next + u_K * (y - u_y_obse);%第一步的估计值 + 修正值；

u_x_estimate = u_x_next_optimal;

%方差的更新

u_p_next_update = u_p_next - u_K * u_pzz * u_K';

u_P = u_p_next_update;

%进行画图程序

x_array = [x_array,x];

e_x_estimate_array = [e_x_estimate_array,e_x_estimate];

p_x_estimate_array = [p_x_estimate_array,p_x_estimate];

u_x_estimate_array = [u_x_estimate_array,u_x_estimate];

e_error(k,:) = abs(x_array(k)-e_x_estimate_array(k));

p_error(k,:) = abs(x_array(k)-p_x_estimate_array(k));

u_error(k,:) = abs(x_array(k)-u_x_estimate_array(k));

end

t = 0 : tf;

figure;

plot(t,x_array,'k.',t,e_x_estimate_array,'r-',t,p_x_estimate_array,'g--',t,u_x_estimate_array,'b:');

set(gca,'FontSize',10);

set(gcf,'color','White');

xlabel('时间步长');% lable --->label 我的神

ylabel('状态');

legend('真实值','EKF估计值','PF估计值','UKF估计值');

figure;

plot(t,x_array,'k.',t,p_x_estimate_array,'g--', t, p_x_estimate_array-1.96*sqrt(P), 'r:', t, p_x_estimate_array+1.96*sqrt(P), 'r:');

set(gca,'FontSize',10);

set(gcf,'color','White');

xlabel('时间步长');% lable --->label 我的神

ylabel('状态');

legend('真实值','PF估计值', '95% 置信区间');

%root mean square 平均值的平方根

e_xrms = sqrt((norm(x_array-e_x_estimate_array)^2)/tf);

disp(['EKF估计误差均方值=',num2str(e_xrms)]);

p_xrms = sqrt((norm(x_array-p_x_estimate_array)^2)/tf);

disp(['PF估计误差均方值=',num2str(p_xrms)]);

u_xrms = sqrt((norm(x_array-u_x_estimate_array)^2)/tf);

disp(['UKF估计误差均方值=',num2str(u_xrms)]);

% plot(t,e_error,'r-',t,p_error,'g--',t,u_error,'b:');

% legend('EKF估计值误差','PF估计值误差','UKF估计值误差');

t = 1 : tf;

figure;

plot(t,e_error,'r-',t,p_error,'g--',t,u_error,'b:');

set(gca,'FontSize',10);

set(gcf,'color','White');

xlabel('时间步长');% lable --->label 我的神

ylabel('状态');

legend('EKF估计值误差','PF估计值误差','UKF估计值误差');

% toc;

文件列表（点击上边下载按钮，如果是垃圾文件请在下面评价差评或者投诉）:

EKF_UKF_PF1.m
EKF要求.docx
KF和EKF和UKF介绍.docx
UKF要求.docx

关键词： KF EKF UKF

matlab源码下载排行

回到顶部

友情链接

matlab源码网站源码编程语言网络编程手机通讯编程 KUNAG 官网 KUNAG 电商更多

: 联系方式| 版权声明| 招聘信息| 广告服务| 银行汇款| 法律顾问| 兼职技术| 付款方式| 关于我们|; 网站客服程序员兼职招聘 411241577@qq.com; 沪ICP备19040327号-3; 公安备案号：沪公网安备 31011802003874号; 库纳格流体控制系统（上海）有限公司版权所有; Copyright © 1999-2014, GUSUCODE.COM, All Rights Reserved

关键词： KF EKF UKF

相关推荐

matlab源码下载排行

友情链接