⭐ 欢迎来到虫虫下载站！ | 📦 资源下载 📁 资源专辑 ℹ️ 关于我们
⭐ 虫虫下载站
📤 上传资源

📄 wu_fangzhen2_kalman.m

📁 这些程序都是经过本人和许许多多工作过学习过的老师同学
💻 M
字号:
🔍 
clc;
clear all;

format long;

fid1= fopen('E:\孙国伟\work\data21.txt','r');
data1=fscanf(fid1,'%f',[18,63001]);

fid2= fopen('E:\孙国伟\work\data22.txt','r');
data2=fscanf(fid2,'%f',[18,63001]);

fid3= fopen('E:\孙国伟\work\data23.txt','r');
data3=fscanf(fid3,'%f',[18,63001]);

fid4= fopen('E:\孙国伟\work\data24.txt','r');
data4=fscanf(fid4,'%f',[18,63001]);

fid5= fopen('E:\孙国伟\work\data25.txt','r');
data5=fscanf(fid5,'%f',[18,63001]);

fid6= fopen('E:\孙国伟\work\data26.txt','r');
data6=fscanf(fid6,'%f',[18,63001]);

fid7= fopen('E:\孙国伟\work\data27.txt','r');
data7=fscanf(fid7,'%f',[18,63001]);

fid8= fopen('E:\孙国伟\work\data28.txt','r');
data8=fscanf(fid8,'%f',[18,63001]);

fid9= fopen('E:\孙国伟\work\data29.txt','r');
data9=fscanf(fid9,'%f',[18,63001]);

fid10= fopen('E:\孙国伟\work\data210.txt','r');
data10=fscanf(fid10,'%f',[18,63001]);

fid11= fopen('E:\孙国伟\work\data211.txt','r');
data11=fscanf(fid11,'%f',[18,63001]);

fid12= fopen('E:\孙国伟\work\data212.txt','r');
data12=fscanf(fid12,'%f',[18,63001]);

fid13= fopen('E:\孙国伟\work\data213.txt','r');
data13=fscanf(fid13,'%f',[18,63001]);

fid14= fopen('E:\孙国伟\work\data214.txt','r');
data14=fscanf(fid14,'%f',[18,63001]);

fid15= fopen('E:\孙国伟\work\data215.txt','r');
data15=fscanf(fid15,'%f',[18,63001]);

fid16= fopen('E:\孙国伟\work\data216.txt','r');
data16=fscanf(fid16,'%f',[18,63001]);

fid17= fopen('E:\孙国伟\work\data217.txt','r');
data17=fscanf(fid17,'%f',[18,63001]);

fid18= fopen('E:\孙国伟\work\data218.txt','r');
data18=fscanf(fid18,'%f',[18,63001]);


%系统的观测矩阵C阵

A=zeros(18,18,63001);

A(1,:,:)=data1;
A(2,:,:)=data2;
A(3,:,:)=data3;
A(4,:,:)=data4;
A(5,:,:)=data5;
A(6,:,:)=data6;
A(7,:,:)=data7;
A(8,:,:)=data8;
A(9,:,:)=data9;
A(10,:,:)=data10;
A(11,:,:)=data11;
A(12,:,:)=data12;
A(13,:,:)=data13;
A(14,:,:)=data14;
A(15,:,:)=data15;
A(16,:,:)=data16;
A(17,:,:)=data17;
A(18,:,:)=data18;

g=9.80;

B=eye(18);

a=zeros(18,18,63001);

for i=1:63001
    
    [a(:,:,i),b]=c2d(A(:,:,i),B,2);
    
end

%系统的观测矩阵C阵

C=zeros(5,18);

C(1,4)=1;
C(2,5)=1;
C(3,1)=1;
C(4,2)=1;
C(5,3)=1;

%kalman滤波方程的定义

estX=zeros(18,127);
realX=zeros(18,127);
PP=zeros(18,18,126);
P=zeros(18,18,127);
K=zeros(18,5,127);
Z=zeros(5,127);
estXX=zeros(18,501);
CP=zeros(18,18,501);

%系统的观测噪声的协方差强度阵

RR=zeros(5);

RR(1,1)=0.04;
RR(2,2)=0.04;
RR(3,3)=(0.1*pi/180)^2;
RR(4,4)=(0.1*pi/180)^2;
RR(5,5)=(0.3*pi/180)^2;

%系统噪声方差阵Q阵

Q=zeros(18);

Q(13,13)=(0.01*pi/(180*3600))^2;
Q(14,14)=(0.01*pi/(180*3600))^2;   
Q(15,15)=(0.01*pi/(180*3600))^2;
Q(16,16)=(2e-4*g)^2;  
Q(17,17)=(2e-4*g)^2;              
Q(18,18)=(2e-4*g)^2; 
 
%系统噪声

w=[0;0;0;0;0;0;0;0;0;0;0;0;0.01*pi/(180*3600)*randn(1);0.01*pi/(180*3600)*randn(1);0.01*pi/(180*3600)*randn(1);2e-4*g*randn(1);2e-4*g*randn(1);2e-4*g*randn(1)];

%量测噪声

v=[0.2*randn(1) 0.2*randn(1) 0.1*pi/180*randn(1) 0.1*pi/180*randn(1) 0.3*pi/180*randn(1)]';


%状态变量的初始值

estX(:,1)=[0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]';

realX(:,1)=[0.1*pi/180 0.1*pi/180 0.3*pi/180 0.2 0.2 2*pi/(60*180) 3*pi/180 3*pi/180 3*pi/180 3.6e-3*g 3.6e-3*g 3.6e-3*g 0.01*pi/(180*3600) 0.01*pi/(180*3600) 0.01*pi/(180*3600) 2e-4*g 2e-4*g 2e-4*g]';

%协方差矩阵的初始值

P(1,1,1)=(0.1*pi/180)^2;
P(2,2,1)=(0.1*pi/180)^2;
P(3,3,1)=(0.3*pi/180)^2;
P(4,4,1)=0.04;
P(5,5,1)=0.04;
P(6,6,1)=(2*pi/(60*180))^2;
P(7,7,1)=(3*pi/180)^2;
P(8,8,1)=(3*pi/180)^2;
P(9,9,1)=(3*pi/180)^2;
P(10,10,1)=(3.6e-3*g)^2;
P(11,11,1)=(3.6e-3*g)^2;
P(12,12,1)=(3.6e-3*g)^2;
P(13,13,1)=(0.01*pi/(180*3600))^2;
P(14,14,1)=(0.01*pi/(180*3600))^2;
P(15,15,1)=(0.01*pi/(180*3600))^2;
P(16,16,1)=(2e-4*g)^2;
P(17,17,1)=(2e-4*g)^2;
P(18,18,1)=(2e-4*g)^2;


CP(:,:,1)=P(:,:,1);

for j=1:500
    
    for i=1:126
        
        a(:,:,i)=a(:,:,126*(j-1)+i);
        PP(:,:,i+1)=a(:,:,i)*P(:,:,i)*a(:,:,i)'+Q;
        K(:,:,i+1)=PP(:,:,i+1)*C'*inv(C*PP(:,:,i+1)*C'+RR);
        realX(:,i+1)=a(:,:,i)*realX(:,i)+w;
        Z(:,i+1)=C*realX(:,i+1)+v;
        estX(:,i+1)=a(:,:,i)*estX(:,i)+K(:,:,i+1)*(Z(:,i+1)-C*a(:,:,i)*estX(:,i));
        P(:,:,i+1)=PP(:,:,i+1)-K(:,:,i+1)*C*PP(:,:,i+1);
        
    end
    
    P(:,:,1)=P(:,:,127);
    realX(:,1)=realX(:,127);
    estX(:,1)=estX(:,127);
    estXX(:,j+1)=estX(:,127);
    CP(:,:,j+1)=P(:,:,127);
    
end

for i=1:501
    
    est1(i)=estXX(1,i)*180/pi;
    est2(i)=estXX(2,i)*180/pi;
    est3(i)=estXX(3,i)*180/pi;
    est4(i)=estXX(4,i);
    est5(i)=estXX(5,i);
    est6(i)=estXX(6,i)*180*60/pi;
    est7(i)=estXX(7,i)*180*3600/pi;
    est8(i)=estXX(8,i)*180*3600/pi;
    est9(i)=estXX(9,i)*180*3600/pi;
    est10(i)=estXX(10,i)/(1e-6*g);
    est11(i)=estXX(11,i)/(1e-6*g);
    est12(i)=estXX(12,i)/(1e-6*g);
    est13(i)=estXX(13,i)*180*3600/pi;
    est14(i)=estXX(14,i)*180*3600/pi;
    est15(i)=estXX(15,i)*180*3600/pi;
    est16(i)=estXX(16,i)/(1e-6*g);
    est17(i)=estXX(17,i)/(1e-6*g);
    est18(i)=estXX(18,i)/(1e-6*g);
    
    PPP1(i)=CP(1,1,i)*(180/pi)^2;
    PPP2(i)=CP(2,2,i)*(180/pi)^2;
    PPP3(i)=CP(3,3,i)*(180/pi)^2;
    PPP4(i)=CP(4,4,i);
    PPP5(i)=CP(5,5,i);
    PPP6(i)=CP(6,6,i);
    PPP7(i)=CP(7,7,i)*(180*3600/pi)^2;
    PPP8(i)=CP(8,8,i)*(180*3600/pi)^2;
    PPP9(i)=CP(9,9,i)*(180*3600/pi)^2;
    PPP10(i)=CP(10,10,i)/(1e-6*g)^2;
    PPP11(i)=CP(11,11,i)/(1e-6*g)^2;
    PPP12(i)=CP(12,12,i)/(1e-6*g)^2;
    PPP13(i)=CP(13,13,i)*(180*3600/pi)^2;
    PPP14(i)=CP(14,14,i)*(180*3600/pi)^2;
    PPP15(i)=CP(15,15,i)*(180*3600/pi)^2;   
    PPP16(i)=CP(16,16,i)/(1e-6*g)^2;
    PPP17(i)=CP(17,17,i)/(1e-6*g)^2;
    PPP18(i)=CP(18,18,i)/(1e-6*g)^2;

end

%画出状态变量的估计值的曲线图

t=0:252/3600:35;

figure(1)

subplot(3,3,1)
plot(t,est1,'b');
xlabel('time/h');
grid on;
subplot(3,3,2)
plot(t,est2,'b');
xlabel('time/h');
grid on;
subplot(3,3,3)
plot(t,est3,'b');
xlabel('time/h');
grid on;
subplot(3,3,4)
plot(t,est4,'b');
xlabel('time/h');
grid on;
subplot(3,3,5)
plot(t,est5,'b');
xlabel('time/h');
grid on;
subplot(3,3,6)
plot(t,est6,'b');
xlabel('time/h');
grid on;
subplot(3,3,7)
plot(t,est7,'b');
xlabel('time/h');
grid on;
subplot(3,3,8)
plot(t,est8,'b');
xlabel('time/h');
grid on;
subplot(3,3,9)
plot(t,est9,'b');
xlabel('time/h');
grid on;

figure(2)

subplot(3,3,1)
plot(t,est10,'b');
xlabel('time/h');
grid on;
subplot(3,3,2)
plot(t,est11,'b');
xlabel('time/h');
grid on;
subplot(3,3,3)
plot(t,est12,'b');
xlabel('time/h');
grid on;
subplot(3,3,4)
plot(t,est13,'b');
xlabel('time/h');
grid on;
subplot(3,3,5)
plot(t,est14,'b');
xlabel('time/h');
grid on;
subplot(3,3,6)
plot(t,est15,'b');
xlabel('time/h');
grid on;
subplot(3,3,7)
plot(t,est16,'b');
xlabel('time/h');
grid on;
subplot(3,3,8)
plot(t,est17,'b');
xlabel('time/h');
grid on;
subplot(3,3,9)
plot(t,est18,'b');
xlabel('time/h');
grid on;


%协方差矩阵的仿真

figure(3)

subplot(3,3,1)
plot(t,PPP1,'b');
xlabel('time/h');
grid on;
subplot(3,3,2)
plot(t,PPP2,'b');
xlabel('time/h');
grid on;
subplot(3,3,3)
plot(t,PPP3,'b');
xlabel('time/h');
grid on;
subplot(3,3,4)
plot(t,PPP4,'b');
xlabel('time/h');
grid on;
subplot(3,3,5)
plot(t,PPP5,'b');
xlabel('time/h');
grid on;
subplot(3,3,6)
plot(t,PPP6,'b');
xlabel('time/h');
grid on;
subplot(3,3,7)
plot(t,PPP7,'b');
xlabel('time/h');
grid on;
subplot(3,3,8)
plot(t,PPP8,'b');
xlabel('time/h');
grid on;
subplot(3,3,9)
plot(t,PPP9,'b');
xlabel('time/h');
grid on;

figure(4)

subplot(3,3,1)
plot(t,PPP10,'b');
xlabel('time/h');
grid on;
subplot(3,3,2)
plot(t,PPP11,'b');
xlabel('time/h');
grid on;
subplot(3,3,3)
plot(t,PPP12,'b');
xlabel('time/h');
grid on;
subplot(3,3,4)
plot(t,PPP13,'b');
xlabel('time/h');
grid on;
subplot(3,3,5)
plot(t,PPP14,'b');
xlabel('time/h');
grid on;
subplot(3,3,6)
plot(t,PPP15,'b');
xlabel('time/h');
grid on;
subplot(3,3,7)
plot(t,PPP16,'b');
xlabel('time/h');
grid on;
subplot(3,3,8)
plot(t,PPP17,'b');
xlabel('time/h');
grid on;
subplot(3,3,9)
plot(t,PPP18,'b');
xlabel('time/h');
grid on;

⌨️ 快捷键说明

复制代码 Ctrl + C
搜索代码 Ctrl + F
全屏模式 F11
切换主题 Ctrl + Shift + D
显示快捷键 ?
增大字号 Ctrl + =
减小字号 Ctrl + -