观测器_观测器_状态观测_状态观测器全维和降维_

%---------------小车轨迹跟踪全维状态观测器：采用四阶龙格库塔--------- %% clear all; clc; close all; Aa = [-2 1;-2 0]; Bb = [0;1];E = [2;2]; A = [0 1;0 0]; B = [0;1]; C = [1 0]; %% 初始化参数 dx0 = 0; dx1 = 0; %编队队形相对位置 dy0 = 0; dy1 = 0; x0 = 0; x1 = 1; %机器人初始位置 y0 = -1.5; y1 = 2; h = 0.125; %时间间隔 t = 0:h:50; kx = 3; ky = 3; v1 = 0.1; %机器人初始速度 theta1 = pi; %机器人初始航向角 %% 领航者坐标（x,y）、航向角、速度、角速度---全过程 Agv0 = 0.5; %领航者角速度 Theta0 = Agv0*t; %领航者角度 V0 = 0.5; %领航者速度 r0 = 0; %领航者速度导数 %% 从机1-5初始化阶段----从机1-5 Theta1 = []; V1 = []; Agv1 = []; r1 = []; ux1 = 0; ux2 = 0; ux3 = 0; uy1 = 0; uy2 = 0; uy3 = 0; zx1 = 0; zx2 = 0; zx3 = 0; zy1 = 0; zy2 = 0; zy3 = 0; X0 = []; Y0 = []; X0(1) = x0; Y0(1) = y0; X1 = []; Y1 = []; X1(1) = x1; Y1(1) = y1; Theta1(1) = theta1; V1(1) = v1; i = 1; figure grid MINOR axis equal xba = [X1(i);V1(i)*cos(Theta1(i))]; yba = [Y1(i);V1(i)*sin(Theta1(i))]; Vhat(1) = V1(i); x = [1 0.1]'; for i = 1:1:length(t)-1 zx1 = X1(i) - X0(i) - dx1 + dx0; zy1 = Y1(i) - Y0(i) - dy1 + dy0; ux1 = -kx*(V1(i)*cos(Theta1(i)) - V0*cos(Theta0(i)) - phi_xy(zx1)); uy1 = -ky*(V1(i)*sin(Theta1(i)) - V0*sin(Theta0(i)) - phi_xy(zy1)); K1x = Aa*xba(:,i)+Bb*ux1+E*X1(i); %% 观测器:对x方向速度观测 采用四阶龙格库塔 K2x = Aa*(xba(:,i)+K1x*h/2)+Bb*ux1+E*X1(i); K3x = Aa*(xba(:,i)+K2x*h/2)+Bb*ux1+E*X1(i); K4x = Aa*(xba(:,i)+K3x*h/2)+Bb*ux1+E*X1(i); xba(:,i+1) = xba(:,i) + h*(K1x+2*K2x+2*K3x+K4x)/6; K1y = Aa*yba(:,i)+Bb*uy1+E*Y1(i); %% 观测器:对y方向速度观测 采用四阶龙格库塔 K2y = Aa*(yba(:,i)+K1y*h/2)+Bb*uy1+E*Y1(i); K3y = Aa*(yba(:,i)+K2y*h/2)+Bb*uy1+E*Y1(i); K4y = Aa*(yba(:,i)+K3y*h/2)+Bb*uy1+E*Y1(i); yba(:,i+1) = yba(:,i) + h*(K1y+2*K2y+2*K3y+K4y)/6; Vhat(i+1) = sqrt((yba(2,i+1))^2+(xba(2,i+1))^2); r1(i) = cos(Theta1(i))*ux1 + sin(Theta1(i))*uy1 + r0; Agv1(i) = ((-sin(Theta1(i))/V1(i))*ux1) + ((cos(Theta1(i))/V1(i))*uy1) + Agv0; V1(i+1) = V1(i) + r1(i)*h; if (V1(i+1) < 0) V1(i+1) = -V1(i+1); end Theta1(i+1) = Theta1(i) + Agv1(i)*h; X0(i+1) = X0(i) + V0*cos(Theta0(i+1))*h; Y0(i+1) = Y0(i) + V0*sin(Theta0(i+1))*h; X1(i+1) = X1(i) + V1(i+1)*cos(Theta1(i+1))*h; Y1(i+1) = Y1(i) + V1(i+1)*sin(Theta1(i+1))*h; % hold on % plot([X1(i),X1(i+1)],[Y1(i),Y1(i+1)],'r-','LineWidth',2) % hold on % plot([X0(i),X0(i+1)],[Y0(i),Y0(i+1)],'k-.','LineWidth',2) % % pause(0.05) end figure plot(t,V1(1:length(t)),'r'); hold on plot(t,Vhat(1:length(t)),'r--'); hold on plot(t,X1(1:length(t)),'b'); hold on plot(t,xba(1,1:length(t)),'b--'); hold on plot(t,Y1(1:length(t)),'m'); hold on plot(t,yba(1,1:length(t)),'m--'); grid minor legend('真实速度','估计速度','真实x','估计x','真实y','估计y'); xlabel('t/s') ylabel('v/(m/s)')