观测器.zip

里面为我设计的全维状态观测器和降维状态观测器仿真程序，为m文件，助于对观测器的理解

%---------------小车轨迹跟踪全维状态观测器：采用四阶龙格库塔---------
%% 
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; %机器人初始航向角

%% 领航者坐标（xy）、航向角、速度、角速度---全过程
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（2i+1））^2+（xba（2i+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（tV1（1:length（t））‘r‘）;
hold on 
plot（tVhat（1:length（t））‘r--‘）;
hold on
plot（tX1（1:length（t））‘b‘）;
hold on 
plot（txba（11:length（t））‘b--‘）;

hold on
plot（tY1（1:length（t））‘m‘）;
hold on 
plot（tyba（11:length（t））‘m--‘）;

grid minor
legend（‘真实速度‘‘估计速度‘‘真实x‘‘估计x‘‘真实y‘‘估计y‘）;
xlabel（‘t/s‘）
ylabel（‘v/（m/s）‘）







   

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2018-12-31 09:08  观测器\
     目录           0  2018-12-30 11:20  观测器\全维状态观测器\
     文件        3126  2018-12-29 16:07  观测器\全维状态观测器\carobserer.m
     文件        1175  2018-12-29 11:21  观测器\全维状态观测器\observer.m
     文件          71  2018-04-03 22:51  观测器\全维状态观测器\phi_xy.m
     文件        1029  2018-12-29 16:11  观测器\全维状态观测器\two.m
     目录           0  2018-12-30 22:55  观测器\降维状态观测器\
     文件        1372  2018-12-30 15:14  观测器\降维状态观测器\Dimensionalstateobserver.m
     文件        1211  2018-12-30 15:17  观测器\降维状态观测器\observer.m