求解非线性方程

#求解非线性方程

求解非线性方程

##牛顿法—更新Jacobian矩阵
待解方程：
###方程引自如下文章：H. Fu and P. Y. Kam, “MAP/ML Estimation of the Frequency and Phase of a Single Sinusoid in Noise,” in IEEE Transactions on Signal Processing, vol. 55, no. 3, pp. 834-845, March 2007, doi: 10.1109/TSP.2006.888055.

###描述：r(k)为正弦波经过AWGN信道后的观测值，待估计量为w和θ。
###代码：以下代码部分参考自：https://blog.csdn.net/weixin_42307478/article/details/113087189

clc;
clear;
close all;

fs = 0.001;
pha = 2;

global A sigma r alpha omega beta deta arc_r N
N = 10;
A = 2;
sigma = 0.2;
alpha = 5;
omega = 1;
beta = 5;
deta = 1;
%% 观测值

for k = 1:N
    %clear r
    r(k) = A*exp(j*(2*pi*fs*(k-1)+pha))   ;

end

noi = randn(1,N)+j*randn(1,N);
% r = r+noi;

for k =1 : N
    %clear arc_r
    arc_r(k) = atan(imag(r(k))/real(r(k)));
end

%% 求解

x0 = [0;1]; % choose the starting value
[x, iter, fval] = fsolveNewton('f' ,x0,1e-6,'dfdx') % user provided Jacobian
xx =x;
save('xdata','xx')
run('MAP_draw.m')

%%
function y = f (x) % define the system of equations

global A sigma r alpha omega beta deta arc_r N

sum1 = 0;
for k = 1:N
    
    sum1 = sum1 + (k-1)*abs(r(k))*sin(arc_r(k)-x(1)*(k-1)-x(2));
end
y(1) = (2*A/sigma^2)*sum1-alpha*sin(x(1)-omega);

sum2 = 0;
for k = 1:N
    sum2 = sum2 + abs(r(k))*sin(arc_r(k)-x(1)*(k-1)-x(2));
end
y(2) = (2*A/sigma^2)*sum2-beta*sin(x(1)-deta);
y = [y(1);y(2)];
end




%% Jacobian

function s = dfdx(x) % Jacobian

global A sigma r alpha omega beta deta arc_r N

sum1 = 0;
for k = 1:N
    sum1 = sum1 + (-(k-1)^2)*abs(r(k))*cos(arc_r(k)-x(1)*(k-1)-x(2));
end
s(1,1) = (2*A/sigma^2)*sum1-alpha*cos(x(1)-omega);

sum2 = 0;
for k = 1:N
    sum2 = sum2 + (-(k-1))*abs(r(k))*cos(arc_r(k)-x(1)*(k-1)-x(2));
end
s(1,2) = (2*A/sigma^2)*sum2;

sum3 = 0;
for k = 1:N
    sum3 = sum3 + (-(k-1))*abs(r(k))*cos(arc_r(k)-x(1)*(k-1)-x(2));
end
s(2,1) = (2*A/sigma^2)*sum3;

sum4 = 0;
for k = 1:N
    sum4 = sum4 + (-1)*abs(r(k))*cos(arc_r(k)-x(1)*(k-1)-x(2));
end
s(2,2) = (2*A/sigma^2)*sum4 - beta*cos(x(2)-deta);

end




function [x, iter, fval] = fsolveNewton(f ,x0 , tolx , dfdx)
  % [x, iter ] = fsolveNewton(f ,x0 , tolx , dfdx)
  % use Newtons method to solve a system of equations f (x)=0,
  % the number of equations and unknowns have to match
  % the Jacobian matrix is either given by the function dfdx or
  % determined with finite difference computations
  %
  % input parameters :
  % f string with the function name
  % function has to return a column vector
  % x0 starting value
  % tolx allowed tolerances ,
  % a vector with the maximal tolerance for each component
  % if tolx is a scalar , use this value for each of the components
  % dfdx string with function name to compute the Jacobian
  %
  % output parameters :
  % x vector with the approximate solution
  % iter number of required iterations
  % it iter >20 then the algorithm did not converge
  if (( nargin < 3)|( nargin >=5)) % 要求输出的参数为3个或4个，dxfx为缺省参数
     usage( 'wrong number of arguments in fsolveNewton(f ,x0 , tolx , dfdx ) ');
  end
  maxit = 20; % maximal number of iterations
  x = x0;
  iter = 0;
  if isscalar ( tolx ) % 如果容差是一个标量，需要将其转化为维度和未知数维度一致，比如有n个未知数，该容差会应用到这n个解中
     tolx = tolx*ones( size (x0 )); 
  end
  dx = 100*abs( tolx ); % 计算步长，容差的100倍，比如容差设置为1e-6时，dx为1e-4，当然需要考虑向量形式
  f0 = feval (f ,x);    % 获得初值;2*1
  m = length (f0);     
  n = length (x);       % n维列向量
  if (n ~= m) % 这里需要满足满秩要求，即含n个未知数的方程组要有同样n个方程才能求解
     error ('number of equations not equal number of unknown')
  end
  if (n ~= length (dx))
     error ('tolerance not correctly specified' )
  end
  jac = zeros (m,n); % 如果非线性方程组有m个方程，每个方程含未知数为n个，那么其雅可比矩阵大小为（m，n）reserve memory for the Jacobian
  done = false ; % Matlab has no ’do until ’
  
  while ~done
       if nargin==4 % 如果输入4个参数，那么会使用提供的雅克比矩阵
         jac = feval (dfdx ,x);
       else % use a finite difference approx for Jacobian，如果没有提供，使用有限差分的方法逼近
          dx = dx/100; % 这里dx步长缩小100倍，记住这里dx是一个n维列向量
%         disp(dx);
    
           for jj= 1:n
               xn = x;   % x
               xn( jj ) = xn(jj) + dx(jj); 
               % 理解为：f'(x)=(f(x+dx)-f(x))/dx，这里f(x)为f0
               jac (: , jj ) = (( feval (f ,xn)-f0 )/dx( jj )); 
%              disp(jac); % 输出查看jac矩阵
           end
       end
       % jac矩阵是随着每次迭代而不断更新，类比单变量的牛顿法，每次迭代都要计算变量在该次迭代的一阶导数，供下一次迭代计算
       % 注意左除符号，jac是(n,n)大小的矩阵，f0是(n,1)大小的向量，同时该结果赋值给dx，起到了变步长的作用，越接近解时步长越小，步长dx随jac\f0变化
       dx = jac\f0 ; % 可以看出jac又是关于dx的函数，加速迭代
       x = x - dx;
    
       % 下面方式是固定步长方式（同时需要注释掉前面代码：dx=dx/100），jac矩阵更新过程中不利于已有信息进行更新
%      tmp = jac\f0;
%      disp(tmp);
%      x = x-tmp;
       iter = iter +1
       f0 = feval (f ,x);  % 重新赋值f0
       if iter >=maxit %||(( abs(dx(1))<abs(tolx(1)))&&( abs(dx(2))<abs(tolx(2)))))
          done = true ;
       end
       if  abs(dx(1))<abs(tolx(1))&& abs(dx(2))<abs(tolx(2))
          done = true ; 
       end
    end
      fval = feval(f,x);
end

###绘图

clc;clear;close all;
fs = 0.001;  % -0.5 -- 0.5
pha = 2;
% global A sigma r alpha omega beta deta N arc_r
A = 2;
N = 10;
sigma = 0.2;
alpha = 5;
omega = 1;
beta = 5;
deta = 1;


for k = 1:N
    r(k) = A*exp(j*(2*pi*fs*(k-1)+pha));     

end

noi = randn(1,N)+j*randn(1,N);
% r = r+noi;

for k =1 : N
    arc_r(k) = atan(imag(r(k))/real(r(k)));
end

syms x y

sum1 = 0;
for k = 1:N
    sum1 = sum1 + (k-1)*abs(r(k))*sin(arc_r(k)-x*(k-1)-y);
end
e1 = (2*A/sigma^2)*sum1-alpha*sin(x-omega);

sum2 = 0;
for k = 1:N
    sum2 = sum2 + abs(r(k))*sin(arc_r(k)-x*(k-1)-y);
end
e2 = (2*A/sigma^2)*sum2-beta*sin(x-deta);

ezplot(e1)
hold on
h = ezplot(e2);
set(h,'Color','k')
legend('f1(w,θ) = 0','f2(w,θ) = 0')
title('F(w,θ) = 0')
xlabel('w');ylabel('θ')
axis([-pi pi -pi pi])
hold on
load('xdata')
stem(xx(1),xx(2))
%%
% function y = f (x) % define the system of equations
% global A sigma r alpha omega beta deta N arc_r
% 
% sum1 = 0;
% for k = 1:N
%     sum1 = sum1 + k*abs(r(k))*sin(arc_r(k)-x(1)*k-x(2));
% end
% y(1) = (2*A/sigma)*sum1-alpha*sin(x(1)-omega);
% 
% sum2 = 0;
% for k = 1:N
%     sum2 = sum2 + abs(r(k))*sin(arc_r(k)-x(1)*k-x(2));
% end
% y(2) = (2*A/sigma)*sum2-beta*sin(x(1)-deta);
% y = [y(1);y(2)];
% end

##交点即为解，显然该非线性方程组有多个解，初始化不同会导致解的不同，近似解为最靠近初始化点的那个解