advanced navigation gps_dr algorithm.m

高速车辆GPS_DR组合系统新算法研究

%问题：
%1、第40行，多写了一个R(i+1),改正见41行
%2、第48行，函数与原函数不一致，第一个变量和第二个变量在“100”前多乘t,第三个变量在“T1”前少乘t，改动见49、50行
%3、第55行和107行，状态变量X和状态变量的分量X同名，将状态变量改名为Xf,见56行和108行
%4、101行，改动见102行.   说明：错误1对滤波没影响，但影响画图，2、3、4对滤波有影响，具体结果我也没试
%构造测量数据%
u=[1 0.9961 0.9841 0.9783 0.8567 0.5668 0.3367 0.2228 0.1887 0.1777 0.17765 0.17789 0.17795 0.17798  0.18895 0.18945 0.19012 0.19745 0.4434 0.5888 0.7485 0.7499 0.7477 0.7489 0.7563 0.7501 0.7589 0.7688 0.7577 0.7865 0.7884  0.7898 0.7878 0.7888 0.7962 0.7902 0.7991 0.8089 0.8078 0.8267 0.8285  0.8299 0.8278 0.8288 0.8363 0.8303 0.8393 0.8489 0.8478 0.8669 0.8682  0.8699 0.8679 0.8689 0.8671 0.8605 0.8693 0.8787 0.8779 0.9001 0.9236 0.9238 0.9220 0.9241 0.9248 0.9239 0.9355 0.9332 0.9374 0.9455 0.9688 0.9566 0.9755 0.9723 0.9711 0.9844 0.9799 0.9889 0.9801 0.9902 0.9956 0.9923 0.9978 0.9946 0.9971 0.9962 0.9981 0.9969 0.9997 0.9973 1.0011 0.9989 0.9998 0.9996 1.0004 0.9998 0.9979 1.0045 0.99978 0.99994 1.0015];
t=0.001;
Q=1e-8;
R=1e-8;
n=101;
m=12;
p=2;
w=random('norm',0,Q,m,n);
v=random('norm',0,R,p,n);
    Ed(1)=0.9340;
    Eq(1)=-0.1446;
    Wr(1)=0.9922;
    G(1)=0.0864; 
    B(1)=0.0164; 
    R(1)=0.3754;%0.0111/(0.0111^2+0.1716^2)=0.3754%
    X(1)=5.8032;%0.1716/(0.0111^2+0.1716^2)=5.8032% 
    To(1)=1.6; 
    n(1)=1.2; 
    H(1)=1.121; 
    Xs(1)=4.9064;%5.078-0.1716=4.9064% 
    Tl(1)=0.8622;
for i=1:100
    Ed(i+1)=(-t/To(i))*(Ed(i)+Xs(i)*R(i)*(-Eq(i))-X(i)*Xs(i)*(u(i)-Ed(i))+100*pi*(1-Wr(i))*Eq(i))+Ed(i)+w(1,i);
    Eq(i+1)=(-t/To(i))*(Eq(i)-Xs(i)*R(i)*(u(i)-Ed(i))+X(i)*Xs(i)*(-Eq(i))-100*pi*(1-Wr(i))*Ed(i))+Eq(i)+w(2,i);
    Wr(i+1)=(t/H(i))*(Ed(i)*((u(i)-Ed(i))*R(i)+X(i)*Eq(i))+Eq(i)*(R(i)*(-Eq(i))-X(i)*(u(i)-Ed(i)))-t*Tl(i)*(Wr(i)^n(i)))+Wr(i)+w(3,i);
    G(i+1)=G(i)+w(4,i); 
    B(i+1)=B(i)+w(5,i); 
    R(i+1)=R(i)+w(6,i); 
    X(i+1)=X(i)+w(7,i); 
    To(i+1)=To(i)+w(8,i); 
    n(i+1)=n(i)+w(9,i); 
    H(i+1)=H(i)+w(10,i); 
    Xs(i+1)=Xs(i)+w(11,i); 
    Tl(i+1)=Tl(i)+w(12,i);
ID(i)=(1/(0.0111^2+0.1716^2))*(0.0111*(u(i)-Ed(i))+0.1716*(-Eq(i)))+0.0864*u(i)+w(1,i);
IQ(i)=(1/(0.0111^2+0.1716^2))*(0.0111*(-Eq(i))-0.1716*(u(i)-Ed(i)))+0.0164*u(i)+w(2,i);
%  x_true(:,i+1)=[Ed(i+1);Eq(i+1);Wr(i+1);G(i+1);B(i+1);R(i+1);R(i+1);X(i+1);To(i+1);n(i+1);H(i+1);Xs(i+1);Tl(i+1)];
 x_true(:,i+1)=[Ed(i+1);Eq(i+1);Wr(i+1);G(i+1);B(i+1);R(i+1);X(i+1);To(i+1);n(i+1);H(i+1);Xs(i+1);Tl(i+1)];
 y_true(:,i)=[ID(i);IQ(i)];
end
%利用构造数据辨识参数%
syms Ed Eq Wr G B R X To n H Xs Tl Edo Eqo Wro u Id Iq;
t=0.01;
% F=[(-t/To)*(Ed+Xs*R*(-Eq)-X*Xs*(u-Ed))+t*100*pi*(1-Wr)*Eq+Ed
% (-t/To)*(Eq-Xs*R*(u-Ed)+X*Xs*(-Eq))-t*100*pi*(1-Wr)*Ed...
%+Eq (t/H)*(Ed*(R*(u-Ed)+X*Eq)+Eq*(R*(-Eq)-X*(u-Ed))-Tl*Wr^n)+Wr G B R X To n H Xs Tl];
F=[(-t/To)*(Ed+Xs*R*(-Eq)-X*Xs*(u-Ed))+100*pi*(1-Wr)*Eq+Ed (-t/To)*(Eq-Xs*R*(u-Ed)+X*Xs*(-Eq))-100*pi*(1-Wr)*Ed+Eq ...
        (t/H)*(Ed*(R*(u-Ed)+X*Eq)+Eq*(R*(-Eq)-X*(u-Ed))-t*Tl*Wr^n)+Wr G B R X To n H Xs Tl];

h=[(R*(u-Ed)+X*(-Eq))+G*u (R*(-Eq)+X*(u-Ed))+B*u];
% X=[Ed Eq Wr G B R X To n H Xs Tl];
Xf=[Ed Eq Wr G B R X To n H Xs Tl];
FF=jacobian(F,Xf);
hh=jacobian(h,Xf);
N=100;
X1=[0.9340 -0.1446 0.9922 0.0864 0.0164 0.3754 5.8032 1.6 1.2 1.121 4.9064 0.8622]';
Po=eye(12)*0.000001;
U=[1 0.9961 0.9841 0.9783 0.8567 0.5668 0.3367 0.2228 0.1887 0.1777 0.17765 0.17789 0.17795 0.17798  0.18895 0.18945 0.19012 0.19745 0.4434  0.5888 0.7485 0.7499 0.7477  0.7489 0.7563 0.7501 0.7589 0.7688 0.7577 0.7865 0.7884  0.7898 0.7878 0.7888 0.7962 0.7902 0.7991 0.8089 0.8078 0.8267 0.8285  0.8299 0.8278 0.8288 0.8363 0.8303 0.8393 0.8489 0.8478 0.8669 0.8682  0.8699 0.8679 0.8689 0.8671 0.8605 0.8693 0.8787 0.8779 0.9001 0.9236 0.9238 0.9220 0.9241 0.9248 0.9239 0.9355 0.9332 0.9374 0.9455 0.9688 0.9566 0.9755 0.9723 0.9711 0.9844 0.9799 0.9889 0.9801 0.9902 0.9956 0.9923 0.9978 0.9946 0.9971 0.9962 0.9981 0.9969 0.9997 0.9973 1.0011 0.9989 0.9998 0.9996 1.0004 0.9998 0.9979 1.0045 0.99978 0.99994 1.0015];
Z=[ID' IQ'];
Q=1e-8;
R=1e-8;
x_filter=zeros(12,N+1);
cova_filter=zeros(12,N+1);
x_filter(:,1)=[0.9340 -0.1446 0.9922 0.0864 0.0164 0.3754 5.8032 1.6 1.2 1.121 4.9064 0.8622]';
cova_filter(:,1)=diag(Po);
for i=1:N
    u=U(i);
    XX=X1';
    Ed=XX(1);
    Eq=XX(2);
    Wr=XX(3);
    G=XX(4); 
    B=XX(5); 
    R=XX(6);
    X=XX(7);
    To=XX(8);
    n=XX(9);
    H=XX(10);
    Xs=XX(11);
    Tl=XX(12);
    P=eval(FF)*Po*(eval(FF))'+Q*eye(12);

    XX=eval(F);
    Ed=XX(1);
    Eq=XX(2);
    Wr=XX(3);
    G=XX(4); 
    B=XX(5); 
    R=XX(6);
    X=XX(7);
    To=XX(8);
    n=XX(9);
    H=XX(10);
    Xs=XX(11);
    Tl=XX(12);
    CC=eval(hh);
%     K=P*eval(hh)'*inv(eval(hh)*P*eval(hh)'+R);
    K=P*eval(hh)'*inv(eval(hh)*P*eval(hh)'+R*eye(2));
    P1=(eye(12)-K*eval(hh))*P;
    %P1=inv(inv(P)+(CC)'*inv(R)*CC);
    %K=P1*(eval(hh))'*inv(R);
    
%      X1=X'+K*[Z(i,:)'-(eval(h))'];
    X1=eval(Xf)'+K*[Z(i,:)'-(eval(h))'];
    Po=P1;
    x_filter(:,i+1)=X1;
    cova_filter(:,i+1)=diag(P1);
    i
end
    X1
figure(1)
plot(x_true(1,:),'r')
hold on
plot(x_filter(1,:),'k');
figure(2)
plot(x_true(2,:),'r')
hold on
plot(x_filter(2,:),'k');
figure(3)
plot(x_true(3,:),'r')
hold on
plot(x_filter(3,:),'k');
figure(4)
plot(x_true(4,:),'r')
hold on
plot(x_filter(4,:),'k');
figure(5)
plot(x_true(5,:),'r')
hold on
plot(x_filter(5,:),'k');
figure(6)
plot(x_true(6,:),'r')
hold on
plot(x_filter(6,:),'k');
figure(7)
plot(x_true(7,:),'r')
hold on
plot(x_filter(7,:),'k');
figure(8)
plot(x_true(8,:),'r')
hold on
plot(x_filter(8,:),'k');
figure(9)
plot(x_true(9,:),'r')
hold on
plot(x_filter(9,:),'k');
figure(10)
plot(x_true(10,:),'r')
hold on
plot(x_filter(10,:),'k');
figure(11)
plot(x_true(11,:),'r')
hold on
plot(x_filter(11,:),'k');
figure(12)
plot(x_true(12,:),'r')
hold on
plot(x_filter(12,:),'k');