FuzzyIMMFilter/fCV_initialization.m at master · SongJgit/FuzzyIMMFilter · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
%% fCV_initialization 滤波初始化及参数设置. CV模型

function [F,G,H,Q,Xg2,P2]=fCV_initialization(T,Z1,Z2,R2,q)

% Z1,Z2         k=1,2 时的直角坐标测量向量
%  F,G,H,Q,Xg2,P2 依次为 状态转移矩阵，增益阵，测量阵，过程噪声阵，
%  k=2 时的状态估计向量，状态估计误差协方差阵

F=[1 T 0 0 0 0
   0 1 0 0 0 0
   0 0 1 T 0 0
   0 0 0 1 0 0
   0 0 0 0 1 T
   0 0 0 0 0 1];

G=[1/2*T^2 T  0       0 0 0
   0       0  1/2*T^2 T 0 0
   0       0  0       0 1/2*T^2 T]';

%B=[1/2*T^2 T 1/2*T^2 T 1/2*T^2 T]';

H=[1 0 0 0 0 0
   0 0 1 0 0 0
   0 0 0 0 1 0];

Q=q^2*[1 0 0
       0 1 0
       0 0 1];

zx1=Z1(1,1);
zy1=Z1(2,1);
zz1=Z1(3,1);
zx2=Z2(1,1);
zy2=Z2(2,1);
zz2=Z2(3,1);
Xg2(:,1)=[zx2
         (zx2-zx1)/T
         zy2
         (zy2-zy1)/T
         zz2
         (zz2-zz1)/T];
P2(:,:)=[R2(1,1)     R2(1,1)/T      R2(1,2)     R2(1,2)/T       R2(1,3)    R2(1,3)/T
         R2(1,1)/T   2*R2(1,1)/T^2  R2(1,2)/T   2*R2(1,2)/T^2   R2(1,3)/T  2*R2(1,3)/T^2
         R2(1,2)     R2(1,2)/T      R2(2,2)     R2(2,2)/T       R2(2,3)    R2(2,3)/T
         R2(1,2)/T   2*R2(1,2)/T^2  R2(2,2)/T   2*R2(2,2)/T^2   R2(2,3)/T  2*R2(2,3)/T^2
         R2(1,3)     R2(1,3)/T      R2(2,3)     R2(2,3)/T       R2(3,3)    R2(3,3)/T
         R2(1,3)/T   2*R2(1,3)/T^2  R2(2,3)/T   2*R2(2,3)/T^2   R2(3,3)/T  2*R2(3,3)/T^2];