C语言算法程序，关于姿态矩阵的算法。

x=g( cosα sinβ sinγ - sinα cosγ )

y=g* cosβ cosα

z=g*(-sinα sinγ - cosα sinβ cosγ )

剩下的你会

这个话题扩展开去就太大了...简单说一下吧

无人机本身是个非常综合性的系统。就基本的核心的飞行控制部分来说，一般包括内环和外环。内环负责控制飞机的姿态，外环负责控制飞机在三维空间的运动轨迹。

高端的无人机，依靠高精度的加速度计和激光陀螺等先进的传感器（现在流行的都是基于捷连惯导而不是平台式），计算维持飞机的姿态。低端的型号则用一些MEMS器件来做姿态估算。但它们的数学原理基本是相同的。具体的算法根据硬件平台的能力，可能采用离散余弦矩阵/四元数/双子样/多子样....

高端的无人机，AHRS/IMU采用的基本都是民航或者军用的著名产品。例如全球鹰的利顿LN-100G/LN-200等。这些系统价格昂贵但精密，内部往往是零锁激光陀螺之类。例如LN-100G的GPS-INS组合，即使丢失GPS，靠惯性器件漂移仍可以控制在120m/min。

低端的无人机就没那么精密讲究了，一般都依赖GPS等定位系统来进行外环控制，内环用MEMS陀螺和加速度计进行姿态估算。

如果把无人机看成一个完整的系统，那么还需要很多其他支持，例如任务规划，地面跟踪等等....

进行无人机编程，得看你具体是指哪方面。如果是飞控系统，你得需要比较扎实的数学知识，对各种矩阵运算/控制率什么的有深刻的了解。如果只是希望现有的带飞控的平台去做一些任务，那么需要根据具体的平台来考虑。有些平台提供了任务编辑器，甚至更灵活的任务脚本。

著作权归作者所有。

首先，你的文件名不能是pid.m，路径也不能含有pid这种matlab内置的关键字。

其次，有两行错误，建议注释掉，对你的结果没影响，因为你没有使用这个数据。

具体正确的代码如下：

clc

clear

kp1=0.6

kp2=0.6

kp3=1.0

ki1=0.8

ki2=0.8

ki3=0.8

kd1=0.2

kd2=0.2

kd3=0.4

e1(1)=0

e2(1)=0

e3(1)=0

u_1=[0 0 0]'

xeta=[1 0 1 0 1 0]'

ts=0.01

for i=2:1000   

    yd(i)=2*sin(i*ts)   

    time(i)=i*ts   

    tspan=[0 ts]   

    para=u_1   

    [tt,xx]=ode45('pid_modle',tspan,xeta,[],para)  

    xeta=xx(length(xx),:)   

    fai(i)=xeta(1)   

    theta(i)=xeta(3)   

    pai(i)=xeta(5)   

    e1(i)=fai(i)-yd(i)   

    e2(i)=theta(i)-yd(i)   

    e3(i)=pai(i)-yd(i)   

    u1(i)=kp1*e1(i)+ki1*(e1(i)+e1(i-1)*ts)+kd1*(e1(i)-e1(i-1))/ts  

    u2(i)=kp2*e2(i)+ki2*(e2(i)+e2(i-1)*ts)+kd2*(e2(i)-e2(i-1))/ts  

    u3(i)=kp3*e3(i)+ki3*(e3(i)+e3(i-1)*ts)+kd3*(e3(i)-e3(i-1))/ts

%     u(i)=[u1(i) u2(i) u3(i)]'  

%     u_1=u(i)

end

% figure 1

plot(time,fai,'-',time,theta,'-.',time,pai,'.',time,yd)

那个函数文件不变，没有错误。

运行结果如下：

希望采纳，设置为满意答案，谢谢。

---