陀螺数据卡尔曼滤波MATLAB程序

Question

有一组陀螺数据，我也建立了AR(3)模型，现在想用MATLAB仿真进行卡尔曼滤波，谁能提供一下MATLAB程序供参考，万分感谢！！！！！

Answer 1

MATLAB参考程序
1. 绘制连续系统零极点图程序
function [p,q]=sjdt(A,B)
%A:系统函数分母多项式系数向量
%B:系统函数分子多项式系数向量
%p:函数返回的系统函数极点位置行向量
%q:函数返回的系统函数零点位置行向量
p=roots(A); %求系统极点
q=roots(B);; %求系统零点
p=p'; %将极点列向量转置为行向量 q=q'; ％将零点列向量转置为行向量 x=max(abs([p q])); %确定纵坐标范围 x=x+0.1;
y=x; %确定横坐标范围 clf
hold on
axis([-x x -y y]); %确定坐标轴显示范围 axis('square')
plot([-x x],[0 0]) %画横坐标轴 plot([0 0],[-y y]) %画纵坐标轴 plot(real(p),imag(p),'x') %画极点
plot(real(q),imag(q),'o') %画零点
title('连续系统零极点图') %标注标题
text(0.2,x-0.2,'虚轴')
text(y-0.2,0.2,'实轴')
2.根据连续系统零极点分布绘制系统频率响应曲线程序
function splxy(f1,f2,k,p,q)
%f1、f2：绘制频率响应曲线的频率范围（即频率起始和终止点，单位为赫兹） %p、q：系统函数极点和零点位置行向量
%k：绘制频率响应曲线的频率取样间隔
p=p';
q=q';
f=f1:k:f2; %定义绘制系统频率响应曲线的频率范围 w=f*(2*pi);
y=i*w;
n=length(p);
m=length(q);
if n==0 %如果系统无极点 yq=ones(m,1)*y;
vq=yq-q*ones(1,length(w));
bj=abs(vq);
ai=1;
elseif m==0 %如果系统无零点 yp=ones(n,1)*y;
vp=yp-p*ones(1,length(w));
ai=abs(vp);
bj=1;
else
yp=ones(n,1)*y;
yq=ones(m,1)*y;
vp=yp-p*ones(1,length(w));
vq=yq-q*ones(1,length(w));
ai=abs(vp);
bj=abs(vq);
end
Hw=prod(bj,1)./prod(ai,1);
plot(f,Hw);
title('连续系统幅频响应曲线')
xlabel('频率w（单位：赫兹）')
ylabel('F(jw)')
3. 绘制离散系统零极点图程序
function ljdt(A,B)
p=roots(A)； %求系统极点
q=roots(B)； %求系统零点
p=p'； %将极点列向量转置为行向量 q=q'； %将零点列向量转置为行向量 x=max(abs([p q 1]))； %确定纵坐标范围
x=x+0.1；
y=x； %确定横坐标范围 clf
hold on
axis([-x x -y y]) %确定坐标轴显示范围 w=0:pi/300:2*pi；
t=exp(i*w)；
plot(t) %画单位园
axis('square')
plot([-x x],[0 0]) %画横坐标轴
plot([0 0],[-y y]) %画纵坐标轴
text(0.1,x,'jIm[z]')
text(y,1/10,'Re[z]')
plot(real(p),imag(p),'x') %画极点
plot(real(q),imag(q),'o') %画零点
title('pole-zero diagram for discrete system') %标注标题
hold off
4.根据离散系统零极点分布绘制系统频率响应曲线程序
function dplxy(k,r,A,B)
p=roots(A); %求极点
q=roots(B); %求零点
figure(1)
ljdt(A,B) %画零极点图
w=0:l*pi/k:r*pi;
y=exp(i*w); %定义单位圆上的k个频率等分点 N=length(p); %求极点个数
M=length(q); %求零点个数
yp=ones(N,1)*y; %定义行数为极点个数的单位圆向量 yq=ones(M,1)*y; %定义行数为零点个数的单位圆向量 vp=yp-p*ones(1,k+1); %定义极点到单位圆上各点的向量 vq=yq-q*ones(1,k+1); %定义零点到单位圆上各点的向量
Ai=abs(vp); %求出极点到单位圆上各点的向量的模 Bj=abs(vq); %求出零点到单位圆上各点的向量的模 Ci=angle(vp); %求出极点到单位圆上各点的向量的相角 Dj=angle(vq); %求出零点到单位圆上各点的向量的相角 fai=sum(Dj,1)-sum(Ci,1); %求系统相频响应
H=prod(Bj,1)./prod(Ai,1); %求系统幅频响应
figure(2)
plot(w,H); %绘制幅频特性曲线
title('离散系统幅频特性曲线')
xlabel('角频率')
ylabel('幅度')
figure(3)
plot(w,fai)
title('离散系统的相频特性曲线')
xlabel('角频率')
ylabel('相位')

Answer 2

我正好现在在做。。。你莫非也是交大的。。。。

追问

我给你发了个私信，私下请教你