這是俺前幾天發(fā)的阿，你可以借鑒一下啦，也是用牛頓法
總共三個函數(shù)，調用snow_whzh

function snow_whzh
syms x1 y1 fi1 x2 y2 fi2 x3 y3 fi3 t ti;
q=[x1;y1;fi1;x2;y2;fi2;x3;y3;fi3];  %變量列表
Q=[fi1-pi*t-pi/4;
   x1-cos(fi1);   %fi1是已知變量，初值為pi/4
   y1-sin(fi1);
   x2-2*cos(fi2)-2*x1;
   y2-2*sin(fi2);
   y1-y2;
   x3-x2-2*cos(fi2);
   y3;
   fi3;];   %Q是約束方程——非線性方程組
tn=input('過程分析的最大時間量tn=');
J=snow_jacobi(Q,q);
disp(q');
for ti=0:tn/10:tn
    Q=subs(Q,t,ti)      
    q=snow_NR(Q,J,q);
    disp(q');
end

function x=snow_NR(Q,J,x)
%用N-R法求解非線性方程組
%其中Q是約束方程向量，J是約束方程雅可比，%X是各個變量組成的向量
m=length(x);
for i=1:m
    x0(i)=1-1/i;
end
x0=x0';
N=200; %最大迭代次數(shù)
k=1;
while k<N
    xx=x0;
    QQ=subs(Q,x(1),x0(1));
    for i=2:m
        QQ=subs(QQ,x(i),x0(i));
    end
    %disp(QQ);
    %QQ=numeric(QQ);
    JJ=subs(J,x(1),x0(1));
    for i=2:m
        JJ=subs(JJ,x(i),x0(i));
    end
    %JJ=numeric(JJ);
    if abs(det(JJ))<eps %JACOBI矩陣奇異時停止計算
        break ;
    end
    x0=x0-JJ\QQ;
    x0=numeric(x0);
    if norm(x0-xx)<eps  %范數(shù) 
        break;
    end
    k=k+1;
end
x=x0;

function A=snow_jacobi(B,q)
%求約束方程的雅可比矩陣
n=length(B);
m=length(q);
for i=1:n
    for j=1:m
        A(i,j)=diff(B(i),q(j));
    end
end