资源简介
前段时间用平方根无迹卡尔曼滤波做了一个观测器算法,之前也找了好多该算法的资料,在CSDN也下载过,但是并不能用,后来自己结合之前下载的代码又上各种论坛,梳理了一下。最终代码可以用了,论文也顺利投出去了。现在分享给大家,顺便赚一点币
代码片段和文件信息
function [xS]=sr_ukf(fstatexShmeaszQR)
n=numel(x); %numer of states
m_z=numel(z); %numer of measurements
k=0;
L=2*n+1;
alpha=0.5;%0.44;%0.5;%0.1;
beta=2.0;%2.0;
m=alpha*alpha*(n+k)-n; %比例系数 为缩放比例,降低系统总的预测误差
gama=1-alpha*alpha+beta;
%%%%%%%%%%%%%%%%%%%分配sigma点权重
Wm=zeros(L1);
Wc=zeros(L1);
Wm(1)=m/(n+m);
Wc(1)=m/(n+m)+gama;
for i=2:L
Wm(i)=1/(2*(n+m));
Wc(i)=1/(2*(n+m));
end
sqR=chol(R);
sqQ=chol(Q);
%------------------------时间更新过程---------------------------------------
%计算sigma点(选择对称采样方法)
I=sqrt(n+m)*S;
x_sigma=zeros(nL);
x_sigma(:1)=x;
for i=2:n+1
x_sigma(:i) = x+I(:i-1);
end
for i=n+2:L
x_sigma(:i) = x-I(:i-n-1);
end
%传播sigma点
r_sigma=zeros(nL);
for i=1:L
r_sigma(:i)=fstate(x_sigma(:i));
end
%计算先验状态
x_next=zeros(n1);
for i=1:L
x_next=x_next+Wm(i)*r_sigma(:i);
end
%qr分解,这一部分是SRUKF算法和UKF算法不同的地方
A=zeros(nL-1);
for i=2:L
A(:i-1)=sqrt(Wc(i))*(r_sigma(:i)-x_next);
end
[~S_xk]=qr([A sqQ]‘0);
%cholupdate更新
S_next=cholupdate(S_xksqrt(Wc(1))*(r_sigma(:1)-x_next));
% % % % % % % % % % % % S_next=cholupdate(S_xkr_sigma(:1)-x_next‘-‘); %已更改2018.12.2
% % % % % % % % % % % % S_next=chol(S_next*S_next‘-((-Wc(1)^(1/2))
- 上一篇:基于MATLAB的三相PWM整流
- 下一篇:自适应滤波算法汇总
相关资源
- 拟合圆matlab代码
- 灰色预测的matlab代码
- 线性规划.非线性规划.多目标规划Ma
- 模拟退火法Matlab代码[比赛已经用过保
- 稀疏表示内含完整的MATLAB代码
- 新安江模型Matlab代码
- 二元序列的Berlekamp-Massey算法的matlab程
- 基于水平集CV模型的图像分割Matlab代码
- 昆明理工大学纠错编码原理及MATLAB实
- 变分法水平集matlab代码
- dijkstra的matlab代码
- BBHE/DSIHE/MMBEBHE代码 matlab版
- 粒子滤波及其原理黄小平随书代码
- 亲测绝对可用!matlab代码 RGB图片转
- MATLAB遗传算法源代码
- Matlab人脸检测及跟踪代码
- 指纹识别代码
- 基于SIFT算法的图像拼接 matlab代码
- Matlab非线性方程求解代码
- 倒立摆matlab代码
- K-L变换matlab代码
- 数学建模国赛题目,代码。圆桌优化
- 节约法车路路径问题VRP的Matlab程序代
- 半监督分类算法源程序-matlab代码
- 基于暗通道先验的图像去雾MATLAB算法
- 数字图像处理维纳滤波Matlab代码分享
- 手写体识别matlab代码
- 元胞自动机代码
- 图像去噪的matlab代码
- 频率切片小波变换MALTAB代码
评论
共有 条评论