卡尔曼滤波（2）------空间目标角度估计（附代码）

基本原理

详情见教材《现代数字信号处理及运用》P244-P258

基本背景

为了测量空间中目标的方位，我们需要知道空间目标的俯仰角和方位角。假设空间中的目标在运动，那么我们如何根据我们的测量值来估计目标的真实方位呢？

原代码

% 卡尔曼滤波用于空间目标的俯仰角和方位角的测量
clc;
close all;
clear all;

%-----参数设置
N=200;         % 时间序列总数
t=0:N-1;       % 仿真时间
I=eye(2);      % 二维系统
b=[0.03;0.05];   % 目标运动速度

%----Q1要远远小于Q2，卡尔曼滤波效果才明显
Q1=[0,0.005;0,0.001];     % 系统状态噪声的方差
Q2=[0.03,0.1;0.7,0.9];    % 观测噪声的方差
v1=sqrt(Q1)*randn(2,N);   % 系统状态噪声
v2=sqrt(Q2)*randn(2,N);   % 观测噪声

% ----------状态方程和观测方程的系数矩阵
F=[1,0;0,1];            % 状态转移矩阵
C=[1,0;0,1];            % 观测矩阵

% -----------初始化
X=zeros(2,N);           % 物体真实的方位角和俯仰角
X(:,1)=[45;180];        % 初始方位角和俯仰角
P0=[10,0;0,1];          % 初始误差
Z=zeros(2,N);           % 观测方程
Z(:,1)=[47;182];        % 初始观测值
X_est=zeros(2,N);       % 卡尔曼估计状态
X_est(:,1)=Z(:,1);      % 位置观测的初始值以此作为滤波器的初始估计状态
P=zeros(2,N);           % 预测状态误差自相关矩阵
P(1,1)=P0(1,1);
P(2,1)=P0(2,2);


%-----开始卡尔曼滤波
for m=2:N
    % 状态方程
    X(:,m)=F*X(:,m-1)+b+v1(:,m);
    % 观测方程
    Z(:,m)=C*X(:,m)+v2(:,m);
    
    %------ 卡尔曼滤波
    X_pre=F*X_est(:,m-1)+b;       % 状态预测 
    P_pre=F*P0*F'+Q1;           % 协方差预测
    
    a=Z(:,m)-C*X_pre;           % 新息计算
    A=C*P_pre*C'+Q2;            % 新息过程的自相关矩阵
    K=P_pre*C'*inv(A);          % 计算卡尔曼增益
    
    X_est(:,m)=X_pre+K*a;       % 状态更新
    P0=(I-K*C)*P_pre;           % 方差更新
    
    
    P(1,m)=P0(1,1);
    P(2,m)=P0(2,2);
end
%误差计算
messure_err_theta=zeros(1,N);% 位移的测量误差
messure_err_fine=zeros(1,N); % 位移的测量误差
kalman_err_theta=zeros(1,N); % 卡尔曼估计的位移与真实位移之间的偏差
kalman_err_fine=zeros(1,N);  % 卡尔曼估计的速度与真实速度之间的偏差
for m=1:N
    messure_err_theta(m)=Z(1,m)-X(1,m);
    messure_err_fine(m)=Z(2,m)-X(2,m);
    kalman_err_theta(m)=X_est(1,m)-X(1,m);
    kalman_err_fine(m)=X_est(2,m)-X(2,m);
end
messure_err_theta=abs( messure_err_theta);
messure_err_fine=abs( messure_err_fine);
kalman_err_theta=abs(kalman_err_theta);
kalman_err_fine=abs( kalman_err_fine);


%-------绘图
figure(1)
plot(t,X(1,:),'-ro',t,Z(1,:),'-b',t,X_est(1,:),'g*');%俯仰角误差
legend('真实值','观测值','卡尔曼滤波值');grid on;
xlabel('采样时间/s');ylabel('误差角度');title('俯仰角');
figure(2)
plot(t,X(2,:),'-ro',t,Z(2,:),'-b',t,X_est(2,:),'g*');%方位角误差
legend('真实值','观测值','卡尔曼滤波值');grid on;
xlabel('采样时间/s');ylabel('误差角度');title('方位角');
figure(3)
plot(t,messure_err_theta,'-ro',t,kalman_err_theta,'-b*');%俯仰角误差
legend('测量的俯仰角误差','卡尔曼估计的俯仰角误差');grid on;
xlabel('采样时间/s');ylabel('误差角度');
figure(4)
plot(t,messure_err_fine,'-ro',t,kalman_err_fine,'-b*');% 方位角误差
legend('测量的方位角误差','卡尔曼估计的方位角误差');grid on;
xlabel('采样时间/s');ylabel('误差角度');

仿真结果

结果分析

可以看出，经过卡尔曼滤波后，能够根据观测数据估计出空间目标的真实位置。