二维singer滤波

二维卡尔曼滤波，使用singer模型。 这是目标作机动时,卡尔曼滤波模型采用singer模型的matlab 程序.

clc;close all;clear;
T = 2; %采样周期
% sigmaQ = 100;
sigmaR = 200;
N = 100;
F=[1 T T^2/2 0 0 0;
   0 1 T  0 0 0;
   0 0 1  0 0 0;
   0 0 0  1 T T^2/2; 
   0 0 0  0 1 T;
   0 0 0  0 0 1];%状态转移矩阵
H = [1 0 0 0 0 0;
    0 0 0 1 0 0];%量测矩阵
R = [sigmaR^2 0;
    0 sigmaR^2];%量测协方差

qq11 = T^5/20;
qq12 = T^4/8;
qq13 = T^3/6;
qq22 = T^3/3;
qq23 = T^2/2;
qq33 = T;
Q=[qq11 qq12 qq13 0    0    0;
   qq12 qq22 qq23 0    0    0;
   qq13 qq23 qq33 0    0    0;
   0    0    0    qq11 qq12 qq13;
   0    0    0    qq12 qq22 qq23;
   0    0    0    qq13 qq23 qq33];%过程噪声协方差

%初始化跟踪曲线
X = track2D（0.1N）;
X = [X（1:2:）;0.1*randn（1N）;X（3:4:）;0.14*randn（1N）];
Z = [X（1:）;X（4:）];
% Z = X;
% X = reshape（X61100）;

% %直角坐标转极坐标
% for i = 1:N
%     d = sqrt（X（1i）^2+X（4i）^2）;
%     sita = atan（X（4i）/X（1i））;
% end
% %极坐标测量误差
% d = d + sigmaR*randn（1N）;
% sita = sita + 0.001*randn（1N）;
% %极坐标转直角坐标
% for i = 1:N
%     Z（1i） = d（1i）*cos（sita（1i））;
%     Z（2i） = d（1i）*sin（sita（1i））;
% end
X = reshape（X61N）;
Z = Z + sigmaR*randn（2N）;
Z = reshape（Z21N）;%测量数据
for i = 1:N
    xtr（i） = Z（11i）;
    ytr（i） = Z（21i）;
end
plot（xtrytr‘.-‘）;
% xe（::1） = Z（::1）;
xe（::2） = [Z（112）（Z（112）-Z（111））/T2*（Z（112）-Z（111））/T^2...
    Z（212）（Z（212）-Z（211））/T2*（Z（212）-Z（211））/T^2]‘;
xp（::1） = zeros（61）;
xp（::2） = zeros（61）;

%滤波协方差初始化
P11=R（11）;
P12=R（11）/T;
P13=R（11）/T^2;
P22=2*R（11）/T^2;
P23=3*R（11）/T^3;
P33=6*R（11）/T^4;
P44=R（22）;
P45=R（22）/T;
P46=R（22）/T^2;
P55=2*R（22）/T^2;
P56=3*R（22）/T^3;
P66=6*R（22）/T^4;
p（::2）=[P11 P12 P13 0   0   0;
    P12 P22 P23 0   0   0;
    P13 P23 P33 0   0   0;
    0   0   0   P44 P45 P46;
    0   0   0   P45 P55 P56;
    0   0   0   P46 P56 P66];

for k = 3:N
    xp（::k） = F*xe（::k-1）;
    a（::k） = Z（::k）-H*xp（::k）;%新息
    pt（::k） = F*p（::k-1）*F‘+Q;
    A（::k） = H*pt（::k）*H‘+R;
    K（::k） = pt（::k）*H‘*inv（A（::k））;
    
    xe（::k） = xp（::k）+K（::k）*a（::k）;
    p（::k） = （eye（6）-K（::k）*H）*pt（::k）;
end
for i = 1:N;
    xtr（i） = xe（11i）;
    ytr（i） = xe（41i）;
end
plot（xtrytr‘.-‘）;
legend（‘真轨迹‘‘测量轨迹‘‘跟踪轨迹‘）; 

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       2318  2018-09-08 10:42  main.m

     文件        531  2018-09-08 10:41  track2D.m

----------- ---------  ---------- -----  ----

                 2849                    2