我在做一个水下航行器的航迹推算仿真，应用卡尔曼滤波进行优化，发现优化后的曲线存在问题。
水下航行器运动模型 V（导航系下速度）=C（姿态转移矩阵）*v(载体系下的速度）
导航系固定在航行器上，方向指向北东地，其方向不随航行器自身状态改变而改变，载体系随航行器转动，存在三个欧拉角。
卡尔曼滤波中，X（k）=A*X(k-1)+Q
Z=H*X+R
采用常速度模型，
所求状态X包含导航系下的位置和速度两个量，分别在XYZ三个方向，因此X是6 *1的矩阵,X=[X; Y; Z; VX; VY; VZ] 建立X导数和X关系，离散化后得到矩阵A；
观测值Z取载体系下的速度，Z=[vx; vy; vz];利用载体系下的速度与导航系下的速度之前的关系，观测矩阵为H=[0，inv(C)]，inv(C)是从导航系到载体系下的姿态转移矩阵；观测矩阵为3*6的矩阵，Z=H*X中，H主要使X中导航系下的位置和速度与H矩阵相乘得到载体系下的速度，即观测值v
matlab代码如下，
clear all;
close all;
clc;
%------------------------------仿真数据生成----------------------------------%
%第一阶段
A=[0:0.01:2; zeros(1,201); zeros(1,201)];
o1=[zeros(1,201); zeros(1,201); zeros(1,201)]; %该阶段沿x轴直线运动
%第二阶段
B=[2.01:0.01:4;zeros(1,200);zeros(1,200)];
o2=[zeros(1,200);
-pi/400:(-pi/400):(-pi/4),(-pi/4):(pi/400):-pi/400;
zeros(1,200)]; %该阶段做下潜运动
%第三阶段
C=[4*ones(1,100); zeros(1,100); zeros(1,100)];
o3=[zeros(1,100);zeros(1,100);2*pi/100:2*pi/100:2*pi]; %该阶段在xoy平面内圆周运动
%第四阶段
D=[4:(-0.01):2.01; zeros(1,200); zeros(1,200)];
o4=[zeros(1,200);
-pi/400:(-pi/400):(-pi/4),(-pi/4):(pi/400):-pi/400;
-pi/400:(-pi/400):(-pi/4),(-pi/4):(pi/400):-pi/400;]; %该阶段继续下潜
%第五阶段
E=[ 2.01:0.01:4; zeros(1,200); (-0.01):(-0.01):(-2)];
o5=[zeros(1,200); pi/800:pi/800:pi/4; zeros(1,200)]; %该阶段上浮
v1=[A,B,C,D,E]; %整合传感器速度数据
ou1=[o1,o2,o3,o4,o5]; %整合传感器欧拉角数据
%----------------------------传感器噪声值生成------------------------------%
ZS1=0.3*randn(3,size(v1,2)); %速度噪声
ZS2=pi/40*randn(3,size(ou1,2)); %欧拉角噪声
v2=v1+ZS1; %加噪声后的速度
ou2=ou1+ZS2; %加噪声后的欧拉角
%----------------------------仿真航迹推算----------------------------------%
V1=[0,0,0]'; %初始化参数
xyz1=[0,0,0]';
XYZ1=[0,0,0]';
for i=1:size(ou1,2);
t=0.01; %时间间隔
v=v1(:,i); %取对应时刻载体系下的x、y、z方向的速度列向量
a=ou1(1,i); %取对应时刻三个欧拉角
b=ou1(2,i);
c=ou1(3,i);
C=[cos(c)*cos(b), cos(c)*sin(b)*sin(a)-cos(a)*sin(c), cos(c)*sin(b)*cos(a)+sin(c)*sin(a);
cos(b)*sin(c), sin(c)*sin(b)*sin(a)-cos(a)*cos(c), sin(c)*sin(b)*cos(a)-cos(c)*sin(a);
-sin(b), cos(b)*sin(a), cos(a)*cos(b)]; %姿态转移矩阵
tempV=C*v; %计算得到对应时刻导航系下的三个方向3*1的速度列向量
tempXYZ=tempV*t; %计算得到对应时间间隔内导航系下的三个方向3*1的位移列向量
xyz1=xyz1+tempXYZ; %累加近似得到该时刻3*1的位移列向量
V1=[V1,tempV]; %串联速度列向量，得到速度矩阵
XYZ1=[XYZ1,xyz1]; %串联位移列向量，得到位移矩阵
end
V1=V1(:,2:size(V1,2));
XYZ1=XYZ1(:,2:size(XYZ1,2)); %去掉第一行初始值
X1=XYZ1(1,:); %取X方向的位移行向量
Y1=XYZ1(2,:); %取Y方向的位移行向量
Z1=XYZ1(3,:); %取Z方向的位移行向量
%---------------------------加噪声的航迹推算-------------------------------%
V2=[0,0,0]'; %初始化参数
xyz2=[0,0,0]';
XYZ2=[0,0,0]';
for i=1:size(ou2,2);
t=0.01; %时间间隔
v=v2(:,i); %取对应时刻载体系下的x、y、z方向的速度列向量
a=ou2(1,i); %取对应时刻三个欧拉角
b=ou2(2,i);
c=ou2(3,i);
C=[cos(c)*cos(b), cos(c)*sin(b)*sin(a)-cos(a)*sin(c), cos(c)*sin(b)*cos(a)+sin(c)*sin(a);
cos(b)*sin(c), sin(c)*sin(b)*sin(a)-cos(a)*cos(c), sin(c)*sin(b)*cos(a)-cos(c)*sin(a);
-sin(b), cos(b)*sin(a), cos(a)*cos(b)];
tempV=C*v; %计算得到对应时刻导航系下的三个方向3*1的速度列向量
tempXYZ=tempV*t; %计算得到对应时间间隔内导航系下的三个方向3*1的位移列向量
xyz2=xyz2+tempXYZ; %累加近似得到该时刻3*1的位移列向量
V2=[V2,tempV]; %串联速度列向量，得到速度矩阵
XYZ2=[XYZ2,xyz2]; %串联位移列向量，得到位移矩阵
end
V2=V2(:,2:size(V2,2));
XYZ2=XYZ2(:,2:size(XYZ2,2));
X2=XYZ2(1,:); %取X方向的位移行向量
Y2=XYZ2(2,:); %取Y方向的位移行向量
Z2=XYZ2(3,:); %取Z方向的位移行向量
%------------------------------卡尔曼滤波----------------------------------%
AA=[zeros(6,3),[eye(3);zeros(3,3)]];
A=expm(AA*0.01);
R=0.1*eye(3);
Q=0.09*eye(6);
I=eye(6); %定义一个单位阵方便计算
p=100*I; %协方差矩阵初值为单位阵
X33=zeros(6,1); %坐标初值
Xk=zeros(6,1);
Z=v2;
for k=2:size(Z,2);
ZZ=Z(:,k);
Xkk=A*Xk;
p=A*p*A'+Q;
a=ou1(1,k-1); %取对应时刻三个欧拉角
b=ou1(2,k-1);
c=ou1(3,k-1);
C=[cos(c)*cos(b), cos(c)*sin(b)*sin(a)-cos(a)*sin(c), cos(c)*sin(b)*cos(a)+sin(c)*sin(a);
cos(b)*sin(c), sin(c)*sin(b)*sin(a)-cos(a)*cos(c), sin(c)*sin(b)*cos(a)-cos(c)*sin(a);
-sin(b), cos(b)*sin(a), cos(a)*cos(b)]; %姿态转移矩阵
NI=ni(C);
H=[zeros(3,3),NI]; %观测矩阵
K=p*H'*inv(H*p*H'+R); %计算增益
Xkk=Xkk+K*(ZZ-H*Xk);
p=(I-K*H)*p; %协方差迭代更新
X33=[X33,Xkk];
Xk=Xkk;
end
X3=X33(1,:);
Y3=X33(2,:);
Z3=X33(3,:);
%---------------------------------绘图------------------------------------%
subplot(2,2,1);plot3(X1,Y1,Z1,'b',X2,Y2,Z2,'r',X3,Y3,Z3,'g'),grid on;
title('航迹曲线');
xlabel('X');ylabel('Y');zlabel('Z');
set(gca,'zdir','reverse');
subplot(2,2,2);plot(1:size(X1,2),X2-X1,'b',1:size(X1,2),X3-X1,'r'),grid on;
legend('加噪声-仿真','卡尔曼-仿真');
title('X方向差值');
ylabel('X');
subplot(2,2,3);plot(1:size(X1,2),Y2-Y1,'b',1:size(X1,2),Y3-Y1,'r'),grid on;
legend('加噪声-仿真','卡尔曼-仿真');
title('Y方向差值');
ylabel('Y');
subplot(2,2,4);plot(1:size(X1,2),Z2-Z1,'b',1:size(X1,2),Z3-Z1,'r'),grid on;
legend('加噪声-仿真','卡尔曼-仿真');
title('Z方向差值');
ylabel('Z');
代码中的ni()是自己写的求逆矩阵函数
画图用的是卡尔曼滤波值和噪声值分别减去仿真值，得到的图像本应是卡尔曼滤波值在X轴和噪声值之间，实际上却并不是，已经被困扰了几天，求各位大神指导啊
