kalman.m.txt

一种基于卡尔曼滤波的自适应交互式多模型算法

%自适应信号处理

clear;

N=100;%the number of Monte_Carlo
SN=400;%the number of sampling

T=0.1;
g=10;
sigma_u=0.01*g;

sigma_R=100;
% sigma_alpha=0.1*pi/180/2;
% sigma_beita=0.1*pi/180/2;
sigma_alpha=0.1*pi/180;
sigma_beita=0.1*pi/180;

e_r_f=zeros(N,SN);%估计误差
e_alpha_f=zeros(N,SN);
e_beita_f=zeros(N,SN);
e_r_p=zeros(N,SN);%预测误差
e_alpha_p=zeros(N,SN);
e_beita_p=zeros(N,SN);
e_r_o=zeros(N,SN);%观测误差
e_alpha_o=zeros(N,SN);
e_beita_o=zeros(N,SN);
e_r_t=zeros(N,SN);%理论误差

m_r_f=zeros(1,SN);%估计均值
m_alpha_f=zeros(1,SN);
m_beita_f=zeros(1,SN);
m_r_p=zeros(1,SN);%预测均值
m_alpha_p=zeros(1,SN);
m_beita_p=zeros(1,SN);
m_r_o=zeros(1,SN);%观测均值
m_alpha_o=zeros(1,SN);
m_beita_o=zeros(1,SN);

sigma_r_f=zeros(1,SN);%估计方差
sigma_alpha_f=zeros(1,SN);
sigma_beita_f=zeros(1,SN);
sigma_r_p=zeros(1,SN);%预测方差
sigma_alpha_p=zeros(1,SN);
sigma_beita_p=zeros(1,SN);
sigma_r_o=zeros(1,SN);%观测方差
sigma_alpha_o=zeros(1,SN);
sigma_beita_o=zeros(1,SN);
sigma_r_t=zeros(1,SN);


%实际初始位置（真值）
x1_r=5000;
y1_r=5000;
z1_r=5000;

%跟踪目标的速度
v_x=150;
v_y=150;
v_z=150;

P=zeros(6,6);
R=zeros(3,3);

A=[eye(3,3) T*eye(3,3);zeros(3,3) eye(3,3)];%转移矩阵/系统矩阵
H=[eye(3,3) zeros(3,3)];%观测矩阵
Q=[T^4/4*eye(3,3) T^3/2*eye(3,3);T^3/2*eye(3,3) T^2*eye(3,3)]*sigma_u^2;%策动噪声协方差矩阵

%蒙特卡罗实验
for i=1:1:N
    i
    
    %===================计算初始值========================
    
    %第一次真值
    R1_r=sqrt(x1_r^2+y1_r^2+z1_r^2);
    alpha1_r=x1_r/R1_r;
    beita1_r=y1_r/R1_r;
    
    %第一次观测值
    R1_o=R1_r+randn(1,1)*sigma_R;
    alpha1_o=alpha1_r+randn(1,1)*sigma_alpha;
    beita1_o=beita1_r+randn(1,1)*sigma_beita;    
    gama1_o=sqrt(1-alpha1_o^2-beita1_o^2);
    
    x1_o=R1_o*alpha1_o;
    y1_o=R1_o*beita1_o;
    z1_o=R1_o*gama1_o;
    
    %第二次真值
    x2_r=x1_r+T*v_x;
    y2_r=y1_r+T*v_y;
    z2_r=z1_r+T*v_z;
    
    %转换到雷达坐标系
    R2_r=sqrt(x2_r^2+y2_r^2+z2_r^2);
    alpha2_r=x2_r/R2_r;
    beita2_r=y2_r/R2_r;
    
    %第二次观测值
    R2_o=R2_r+randn(1,1)*sigma_R;
    alpha2_o=alpha2_r+randn(1,1)*sigma_alpha;
    beita2_o=beita2_r+randn(1,1)*sigma_beita;
    gama2_o=sqrt(1-alpha2_o^2-beita2_o^2);
    
    x2_o=R2_o*alpha2_o;
    y2_o=R2_o*beita2_o;
    z2_o=R2_o*gama2_o;
    
    %初始位置及速度
    x0=x2_o;
    y0=y2_o;
    z0=z2_o;
    
    v_x0=(x2_o-x1_o)/T;
    v_y0=(y2_o-y1_o)/T;
    v_z0=(z2_o-z1_o)/T;
    
    %初始协方差矩阵P
    P(1,1)=alpha2_o^2*sigma_R^2+R2_o^2*sigma_alpha^2;
    P(1,2)=alpha2_o*beita2_o*sigma_R^2;
    P(2,1)=P(1,2);
    P(1,3)=alpha2_o*gama2_o*sigma_R^2-R2_o^2*alpha2_o*sigma_alpha^2/gama2_o;
    P(3,1)=P(1,3);
    P(1,4)=P(1,1)/T;
    P(4,1)=P(1,4);
    P(1,5)=P(1,2)/T;
    P(5,1)=P(1,5);
    P(1,6)=P(1,3)/T;
    P(6,1)=P(1,6);
    P(2,2)=beita2_o^2*sigma_R^2+R2_o^2*sigma_beita^2;
    P(2,3)=beita2_o*gama2_o*sigma_R^2-R2_o^2*beita2_o*sigma_beita^2/gama2_o;
    P(3,2)=P(2,3);
    P(2,4)=P(1,2)/T;
    P(4,2)=P(2,4);
    P(2,5)=P(2,2)/T;
    P(5,2)=P(2,5);
    P(2,6)=P(2,3)/T;
    P(6,2)=P(2,6);
    P(3,3)=gama2_o^2*sigma_R^2+R2_o^2*((alpha2_o/gama2_o)^2*sigma_alpha^2+(beita2_o/gama2_o)^2*sigma_beita^2);
    P(3,4)=P(1,3)/T;
    P(4,3)=P(3,4);
    P(3,5)=P(2,3)/T;
    P(5,3)=P(3,5);
    P(3,6)=P(3,3)/T;
    P(6,3)=P(3,6);
    P(4,4)=((alpha2_o^2+alpha1_o^2)*sigma_R^2+(R1_o^2+R2_o^2)*sigma_alpha^2)/T/T;
    P(4,5)=(alpha2_o*beita2_o+alpha1_o*beita1_o)*sigma_R^2/T/T;
    P(5,4)=P(4,5);
    P(4,6)=((alpha2_o*gama2_o+alpha1_o*gama1_o)*sigma_R^2-(R2_o^2*alpha2_o/gama2_o+R1_o^2*alpha1_o/gama1_o)*sigma_alpha^2)/T/T;
    P(6,4)=P(4,6);
    P(5,5)=((beita2_o^2+beita1_o^2)*sigma_R^2+(R2_o^2+R1_o^2)*sigma_beita^2)/T/T;
    P(5,6)=((beita2_o*gama2_o+beita1_o*gama1_o)*sigma_R^2-(R2_o^2*beita2_o/gama2_o+R1_o^2*beita1_o/gama1_o)*sigma_beita^2)/T/T;
    P(6,5)=P(5,6);
    P(6,6)=((gama2_o^2+gama1_o^2)*sigma_R^2+((R2_o*alpha2_o/gama2_o)^2+(R1_o*alpha1_o/gama1_o)^2)*sigma_alpha^2+((R2_o*beita2_o/gama2_o)^2+(R1_o*beita1_o/gama1_o)^2)*sigma_beita^2)/T/T;
    
    P=eye(6,6)*1000;
 
    %===================================================
    %============Kalman filter==========================
    for k=1:1:SN
        
        if k==1
            xk=[x0 y0 z0 v_x0 v_y0 v_z0]';
            %xk=[1000,1000,1000,100,100,100]';
            Pk=P;
            xk_r=x2_r;
            yk_r=y2_r;
            zk_r=z2_r;
        end
        
        %the real value
        xk_r=xk_r+v_x*T;
        yk_r=yk_r+v_y*T;
        zk_r=zk_r+v_z*T;
        r_r=sqrt(xk_r*xk_r+yk_r*yk_r+zk_r*zk_r);
        alpha_r=xk_r/r_r;
        beita_r=yk_r/r_r;
        
        %the observe value
        r_o=r_r+randn(1,1)*sigma_R;
        alpha_o=alpha_r+randn(1,1)*sigma_alpha;
        beita_o=beita_r+randn(1,1)*sigma_beita;
        gama_o=sqrt(1-alpha_o*alpha_o-beita_o*beita_o);
        x_o=r_o*alpha_o;
        y_o=r_o*beita_o;
        z_o=r_o*gama_o;       
        
        %the first step
        xk_e=A*xk;
        %the second step
        Pk_e=A*Pk*A'+Q;
        %the third step  
        r=sqrt(xk_e(1)*xk_e(1)+xk_e(2)*xk_e(2)+xk_e(3)*xk_e(3));
        alpha=xk_e(1)/r;
        beita=xk_e(2)/r;
        gama=xk_e(3)/r;
        
        R(1,1)=(alpha*sigma_R)*(alpha*sigma_R)+(r*sigma_alpha)*(r*sigma_alpha);
        R(1,2)=alpha*beita*sigma_R*sigma_R;
        R(2,1)=R(1,2);
        R(1,3)=alpha*gama*sigma_R*sigma_R-r*r*(alpha/gama)*sigma_alpha*sigma_alpha;
        R(3,1)=R(1,3);
        R(2,2)=(beita*sigma_R)*(beita*sigma_R)+(r*sigma_beita)*(r*sigma_beita);
        R(2,3)=beita*gama*sigma_R*sigma_R-r*r*(beita/gama)*sigma_beita*sigma_beita;
        R(3,2)=R(2,3);
        R(3,3)=(gama*sigma_R)*(gama*sigma_R)+(r*(alpha/gama)*sigma_alpha)*(r*(alpha/gama)*sigma_alpha)+(r*(beita/gama)*sigma_beita)*(r*(beita/gama)*sigma_beita);
        
        K=Pk_e*H'*inv(H*Pk_e*H'+R);
        %the forth step
        xk=xk_e+K*([x_o y_o z_o]'-H*xk_e);
        %the fifth step
        Pk=(eye(6,6)-K*H)*Pk_e;  
        %求误差
        r_f=sqrt(xk(1)*xk(1)+xk(2)*xk(2)+xk(3)*xk(3));
        alpha_f=xk(1)/r_f;
        beita_f=xk(2)/r_f;
        r_p=sqrt(xk_e(1)*xk_e(1)+xk_e(2)*xk_e(2)+xk_e(3)*xk_e(3));
        alpha_p=xk_e(1)/r_p;
        beita_p=xk_e(2)/r_p;
    
        e_r_f(i,k)=(r_r-r_f);%滤波
        e_alpha_f(i,k)=(alpha_r-alpha_f);
        e_beita_f(i,k)=(beita_r-beita_f);
        e_r_p(i,k)=(r_r-r_p);%预测
        e_alpha_p(i,k)=(alpha_r-alpha_p);
        e_beita_p(i,k)=(beita_r-beita_p);
        e_r_o(i,k)=(r_r-r_o);%观测
        e_alpha_o(i,k)=(alpha_r-alpha_o);
        e_beita_o(i,k)=(beita_r-beita_o);
        e_r_t(i,k)=Pk(1,1)+Pk(2,2)+Pk(3,3);
    end
end

for k=1:1:SN
    for i=1:1:N
        m_r_f(1,k)=m_r_f(1,k)+e_r_f(i,k);
        m_alpha_f(1,k)=m_alpha_f(1,k)+e_alpha_f(i,k);
        m_beita_f(1,k)=m_beita_f(1,k)+e_beita_f(i,k);
        m_r_p(1,k)=m_r_p(1,k)+e_r_p(i,k);
        m_alpha_p(1,k)=m_alpha_p(1,k)+e_alpha_p(i,k);
        m_beita_p(1,k)=m_beita_p(1,k)+e_beita_p(i,k);
        m_r_o(1,k)=m_r_o(1,k)+e_r_o(i,k);
        m_alpha_o(1,k)=m_alpha_o(1,k)+e_alpha_o(i,k);
        m_beita_o(1,k)=m_beita_o(1,k)+e_beita_o(i,k);
    end
    m_r_f(1,k)=m_r_f(1,k)/N;
    m_alpha_f(1,k)=m_alpha_f(1,k)/N;
    m_beita_f(1,k)=m_beita_f(1,k)/N;
    m_r_p(1,k)=m_r_p(1,k)/N;
    m_alpha_p(1,k)=m_alpha_p(1,k)/N;
    m_beita_p(1,k)=m_beita_p(1,k)/N;
    m_r_o(1,k)=m_r_o(1,k)/N;
    m_alpha_o(1,k)=m_alpha_o(1,k)/N;
    m_beita_o(1,k)=m_beita_o(1,k)/N;
end

for k=1:1:SN
    for i=1:1:N
        sigma_r_f(1,k)=sigma_r_f(1,k)+(e_r_f(i,k)-m_r_f(1,k))*(e_r_f(i,k)-m_r_f(1,k));
        sigma_alpha_f(1,k)=sigma_alpha_f(1,k)+(e_alpha_f(i,k)-m_alpha_f(1,k))*(e_alpha_f(i,k)-m_alpha_f(1,k));
        sigma_beita_f(1,k)=sigma_beita_f(1,k)+(e_beita_f(i,k)-m_beita_f(1,k))*(e_beita_f(i,k)-m_beita_f(1,k));
        sigma_r_p(1,k)=sigma_r_p(1,k)+(e_r_p(i,k)-m_r_p(1,k))*(e_r_p(i,k)-m_r_p(1,k));
        sigma_alpha_p(1,k)=sigma_alpha_p(1,k)+(e_alpha_p(i,k)-m_alpha_p(1,k))*(e_alpha_p(i,k)-m_alpha_p(1,k));
        sigma_beita_p(1,k)=sigma_beita_p(1,k)+(e_beita_p(i,k)-m_beita_p(1,k))*(e_beita_p(i,k)-m_beita_p(1,k));
        sigma_r_o(1,k)=sigma_r_o(1,k)+(e_r_o(i,k)-m_r_o(1,k))*(e_r_o(i,k)-m_r_o(1,k));
        sigma_alpha_o(1,k)=sigma_alpha_o(1,k)+(e_alpha_o(i,k)-m_alpha_o(1,k))*(e_alpha_o(i,k)-m_alpha_o(1,k));
        sigma_beita_o(1,k)=sigma_beita_o(1,k)+(e_beita_o(i,k)-m_beita_o(1,k))*(e_beita_o(i,k)-m_beita_o(1,k));
        sigma_r_t(1,k)=sigma_r_t(1,k)+e_r_t(i,k);
    end
    sigma_r_f(1,k)=sigma_r_f(1,k)/N;
    sigma_alpha_f(1,k)=sigma_alpha_f(1,k)/N;
    sigma_beita_f(1,k)=sigma_beita_f(1,k)/N;
    sigma_r_p(1,k)=sigma_r_p(1,k)/N;
    sigma_alpha_p(1,k)=sigma_alpha_p(1,k)/N;
    sigma_beita_p(1,k)=sigma_beita_p(1,k)/N;
    sigma_r_o(1,k)=sigma_r_o(1,k)/N;
    sigma_alpha_o(1,k)=sigma_alpha_o(1,k)/N;
    sigma_beita_o(1,k)=sigma_beita_o(1,k)/N;
    sigma_r_t(1,k)=sigma_r_t(1,k)/N;
end

%画图
figure
k=(1:1:SN)';
plot(k,m_r_f,'k-.',k,m_r_p,'r--',k,m_r_o,'b-')
xlabel('k')
ylabel('Means Error')
title('Means Error of R')
legend('Rf(k)','Rp(k)','Ro(k)')

%figure(2)
%k=(1:1:300)';
%plot(k,sigma_r_f,'k-.',k,sigma_r_p,'r--',k,sigma_r_o,'b-')
%xlabel('k')
%ylabel('RMSE ')
%title('RMSE of R')
%legend('Rf(k)','Rp(k)','Ro(k)')

figure
k=(1:1:SN)';
plot(k,sigma_r_f,'k-.',k,sigma_r_p,'r--',k,sigma_r_o,'b-',k,sigma_r_t,'g:');
xlabel('k')
ylabel('RMSE')
title('RMSE of R')
legend('Rf(k)','Rp(k)','Ro(k)','Rt(k)')