deltAlpha_S.rar_卡尔曼滤波_导航_惯性导航_组合导航_组合导航滤波

clc; clear; %采样周期T=1s T = 1; %% %生成运动轨迹 %%*******************************************************% %&第一段路，沿x轴匀速直线运动3000m，速度30m/s； %%*******************************************************% s1=3000; v1=30; nt=s1/v1; n1=floor(nt); v1x = repmat(30,1,n1); v1y = repmat(0,1,n1); %角度alpha，deltAlpha为0 alpha1 = zeros(1,n1); deltAlpha1 = zeros(1,n1); t1=[1:nt]; x1t=v1*t1; y1t=0*t1; %%*****************************************************% %%第二段路径，沿一个弧形转弯，角度变化由0到3/4*pi，半径50m; %%弧长，即运动距离s=3/4*pi*r; %%*****************************************************% r = 50; v2 = 30; s2 = 3/4*pi*50; t = s2/v2; n2=round(t); t2=[n1+1:(n2+n1)]; %匀速转弯角速度值d_deltAlpha=3*pi/(4*t2); d_deltAlpha = 3*pi/(4*t); deltAlpha2 = repmat(d_deltAlpha*T,1,n2); alpha2(1) = 0+T*deltAlpha2(1); for i=2:n2 alpha2(i) = alpha2(i-1) + T*deltAlpha2(i); end v2x = v2*cos(alpha2); v2y = v2*sin(alpha2); x2t = s1+r*cos(3*pi/(4*3.927).*(t2-n1)-pi/2); y2t = r+r*sin(3*pi/(4*3.927).*(t2-n1)-pi/2); %%*************************************************% %第三段路，沿3*pi/4方向直线运动，1500m，速度30m/s； %%*************************************************% s3 = 1500; v3 = 30; t = s3/v3; n3 = floor(t); t3 = [(n2+n1+1):(n2++n1+n3)]; deltAlpha3 = zeros(1,t); alpha3 = repmat(3*pi/4,1,t); v3x = v3.*cos(alpha3); v3y = v3.*sin(alpha3); x3t(1) = x2t(n2)+v3x(1)*T; y3t(1) = y2t(n2)+v3y(1)*T; for i=2:n3 x3t(i) = x3t(i-1)+v3x(i)*T; y3t(i) = y3t(i-1)+v3y(i)*T; end %%*************************************************% %%合并三段运动轨迹； %%*************************************************% x = [x1t x2t x3t]; y = [y1t y2t y3t]; N = size(x,2); alpha = [alpha1 alpha2 alpha3]; deltAlpha = [deltAlpha1 deltAlpha2 deltAlpha3]; deltS = repmat(30,1,N); % plot(x,y) % ylim([-1500 1500]); %% %生成噪声 %%********************************% %%接收机定位误差均值为0，均方差为10的噪声 %%********************************% sigma = 10; xErr = normrnd(0,sigma,1,N); yErr = normrnd(0,sigma,1,N); %%*********************************************************************% %%测姿仪航向角误差alpha：均值为0，均方差为2*pi/(2*360)弧度（0.5度）； %%陀螺仪角度变化量deltAlpha误差：均值为0，均方差为2*pi/(2*3600)弧度（0.05度）； %%*********************************************************************% sigma = 2*pi/(2*360); alphaErr = normrnd(0,sigma,1,N); sigma = 2*pi/(2*3600); deltAlphaErr = normrnd(0,sigma,1,N); sigma_deltS = 1; %% xObserve = x+xErr; yObserve = y+yErr; alphaObserve = alpha+alphaErr; deltAlphaObserve = deltAlpha+deltAlphaErr; deltSObserve = deltS+normrnd(0,sigma_deltS,1,N); figure (1) plot(x,y,'g'); hold on; plot(xObserve,yObserve); % figure(2) % plot(1:N,alphaObserve); % figure(3) % plot(1:N,deltAlphaObserve); %% %卡尔曼滤波处理 %%************************************************************************% %%建模，状态方程：Xk = A*Xk_1 + B*Wk； %%观测方程Zk = h(Xk,v) + Vk（观测方程非线性模型）-->Z=H*Xk+Vk； %%状态量X=[x,vx,ax,y,vy,ay];观测量Z=[xObserver,yObserver,alpha,deltAlpha]; %%************************************************************************% sigma_a = 0.2; sigma_x = 7; sigma_y = 7; sigma_v = 0.7; sigma_alpha = 2*pi/(2*360); sigma_deltAlpha = 2*pi/(2*3600); % sigma_alpha = 0.5; % sigma_deltAlpha = 0.1; A = [1 T 1/2*T^2 0 0 0; 0 1 T 0 0 0; 0 0 1 0 0 0; 0 0 0 1 T 1/2*T^2; 0 0 0 0 1 T; 0 0 0 0 0 1]; % Q = [0 0 0 0 0 0; % 0 0 0 0 0 0; % 0 0 sigma_a^2 0 0 0; % 0 0 0 0 0 0; % 0 0 0 0 0 0; % 0 0 0 0 0 sigma_a^2]; % Q = [1/10*sigma_a 1/8*sigma_a 1/6*sigma_a 0 0 0 % 1/8*sigma_a 1/3*sigma_a 1/2*sigma_a 0 0 0 % 1/6*sigma_a 1/2*sigma_a sigma_a 0 0 0 % 0 0 0 1/10*sigma_a 1/8*sigma_a 1/6*sigma_a % 0 0 0 1/8*sigma_a 1/3*sigma_a 1/2*sigma_a % 0 0 0 1/6*sigma_a 1/2*sigma_a sigma_a]; Q = [1/10*sigma_x 1/8*sigma_v 1/6*sigma_a 0 0 0 1/8*sigma_a 1/3*sigma_a 1/2*sigma_a 0 0 0 1/6*sigma_a 1/2*sigma_a sigma_a 0 0 0 0 0 0 1/10*sigma_a 1/8*sigma_a 1/6*sigma_a 0 0 0 1/8*sigma_a 1/3*sigma_a 1/2*sigma_a 0 0 0 1/6*sigma_a 1/2*sigma_a sigma_a]; R = [sigma_x^2 0 0 0 0 sigma_y^2 0 0 0 0 sigma_deltAlpha^2 0 0 0 0 sigma_deltS^2]; %%**********************************************************************% %%滤波过程 %%**********************************************************************% X{1} = [xObserve(1) 30 0 yObserve(1) 0 0]'; XHat{1} = X{1}; Xklf{1} = XHat{1}; P{1} = zeros(6,6); P{1} = eye(6); W = zeros(6,6); W(3,3) = 1; W(6,6) = 1; V = eye(4); H{1} = zeros(4,6); H{1}(1,1) = 1; H{1}(2,4) = 1; H{1}(3,2) = (X{1}(6)*(X{1}(2)^2+X{1}(5)^2)-(X{1}(2)*X{1}(6)-X{1}(5)*X{1}(3))*2*X{1}(3))/(X{1}(2)^2+X{1}(5)^2)^2; H{1}(3,3) = (-X{1}(5)*(X{1}(2)^2+X{1}(5)^2))/(X{1}(2)^2+X{1}(5)^2)^2; H{1}(3,5) = (-X{1}(3)*(X{1}(2)^2+X{1}(5)^2)-(X{1}(2)*X{1}(6)-X{1}(5)*X{1}(3))*2*X{1}(5))/(X{1}(2)^2+X{1}(5)^2)^2; H{1}(3,6) = (X{1}(2)*(X{1}(2)^2+X{1}(5)^2))/(X{1}(2)^2+X{1}(5)^2)^2; H{1}(4,2) = X{1}(2)/sqrt(X{1}(2)^2+X{1}(5)^2); H{1}(4,5) = X{1}(5)/sqrt(X{1}(2)^2+X{1}(5)^2); for i=2:N XHat{i} = A*Xklf{i-1}; P_k{i} = A*P{i-1}*A'+W*Q*W'; %更新H矩阵 H{i} = zeros(4,6); H{i}(1,1) = 1; H{i}(2,4) = 1; H{i}(3,2) = (XHat{i}(6)*(XHat{i}(2)^2+XHat{i}(5)^2)-(XHat{i}(2)*XHat{i}(6)-XHat{i}(5)*XHat{i}(3))*2*XHat{i}(3))/(XHat{i}(2)^2+XHat{i}(5)^2)^2; H{i}(3,3) = -XHat{i}(5)/(XHat{i}(2)^2+XHat{i}(5)^2); H{i}(3,5) = (-XHat{i}(3)*(XHat{i}(2)^2+XHat{i}(5)^2)-(XHat{i}(2)*XHat{i}(6)-XHat{i}(5)*XHat{i}(3))*2*XHat{i}(5))/(XHat{i}(2)^2+XHat{i}(5)^2)^2; H{i}(3,6) = XHat{i}(2)/(XHat{i}(2)^2+XHat{i}(5)^2); H{i}(4,2) = XHat{i}(2)/sqrt(XHat{i}(2)^2+XHat{i}(5)^2); H{i}(4,5) = XHat{i}(5)/sqrt(XHat{i}(2)^2+XHat{i}(5)^2); K{i} = P_k{i}*(H{i})'*inv(H{i}*P_k{i}*H{i}'+V*R*V'); b{i}(1,1) = xObserve(i)-XHat{i}(1,1); b{i}(2,1) = yObserve(i)-XHat{i}(4,1); b{i}(3,1) = deltAlphaObserve(i)-(XHat{i}(2,1)*XHat{i}(6,1)-XHat{i}(5,1)*XHat{i}(3,1))/(XHat{i}(2,1)^2+XHat{i}(5,1)^2); b{i}(4,1) = deltSObserve(i)-T*sqrt(XHat{i}(2)^2+XHat{i}(5)^2); Xklf{i} = XHat{i}+K{i}*b{i}; P{i} = (eye(6)-K{i}*H{i})*P_k{i}; end for i=1:N xklf(i) = Xklf{i}(1,1); yklf(i) = Xklf{i}(4,1); end hold on; plot(xklf,yklf,'r'); legend('真实轨迹','观测值','滤波值'); for i=1:N err1(i) = sqrt((xObserve(i)-x(i))^2+(yObserve(i)-y(i))^2); err2(i) = sqrt((xklf(i)-x(i))^2+(yklf(i)-y(i))^2); end figure(2) plot(1:N,err1); hold on; plot(1:N,err2,'r'); legend('观测值均方误差','滤波值均方误差');