线性化即将非线性方程转化为线性方程的过程。主流方法是在给定点对非线性方程进行线性微分逼近；需要注意的是它的逼近条件，即需要在变量很小的情况下才能成立。（第一次写微博，编辑界面还不大会用，公式就直接插入图片了，哈哈…）!
令状态变量为：
对状态变量进行一阶泰勒近似（f(seita_k1)=f(seita_k0)+(seita_k1-seita_k0)f（seita_k0）’，因变量为seita,k时刻到k+1时刻因变量增加了w个角度，即seita_k1-seita_k0=w）得：
对上式求偏导，得到雅克比矩阵为：
观测量为x、y，则观测H矩阵为：
Q为状态线性化估计噪声，R为观测噪声。（实际很多观测传感器比状态估计误差要大）
每次迭代都会进行线性化处理，同时也带入了误差，就像惯性导航一样误差不断漂移。这样迭代时间长了误差必然越来越大，在没有及时纠正的情况下就出现发散的情况，运动时间越长迭代发散情况就特别明显。所以在每次线性化近似时，都要进行卡尔曼估计以减小因线性近似时而带入的误差累积，实现对线性近似迭代过程的纠正，即每次迭代结束后的卡尔曼估计值作为下一次迭代线性化近似的初始值。
仿真结果显示扩展卡尔曼滤波结果并不理想，和真实轨迹相差较大，这是因为非线性方程线性化过程中已经引入误差（fig.2也说明线性近似只能在某个点进行，而不能应用到整个点，不断迭代近似而没有纠正过程必然导致发散），当观测量误差偏大一些时，单一的扩展卡尔曼并不能取得较好的结果，后续还需要进行拟合、差值、光滑等处理，而这些处理也需要一些工程经验值，有时间再把后处理加上，先就这些吧。（观测误差改的很小的时候也能取得个好的结果，也就是卡尔曼滤波向观测值倾向，但实际观测误差不可能这么小，否则卡尔曼滤波也失去了意义，如下图所示）
%% =================================================
%% EKF滤波 线性化近似模拟程序
%% 只有在相邻或相近才可以使用线性近似;
%% 迭代使用必然会使误差增大，出现发散;
%% 此时就需要卡尔曼滤波进行迭代预测减小每次线性化的误差
%% Author@xclRobotLife（凌寒画家）2019.1.15 modification
%% =======================模型=======================
%% x=5cos(seita)
%% y=5sin(seita)
clc;clear;
N=100;
w=2*pi/(N);
X=zeros(4,N);
X(:,1)=[5 0 0 w];%初始值
x(1)=5;
y(1)=0;
for t=1:N
seita=w*t;
dx=-5*w*sin(seita);%一阶泰勒近似x=x+wdx 角度每次变化w
ddx=-5*w*cos(seita);%再次泰勒近似，雅克比矩阵计算
dxw=-5*sin(seita);
dy=5*w*cos(seita);
ddy=-5*w*sin(seita);
dyw=5*cos(seita);
F=[1 0 ddx dxw;0 1 ddy dyw;0 0 1 1;0 0 0 1];%雅克比矩阵
if t>1
X(:,t)=F*X(:,t-1);%迭代线性化结果
x(t)=5*cos(seita);%真实值
y(t)=5*sin(seita);
end
end
figure(1)
plot(x(1),y(1),'og'),hold on;
plot(x(N),y(N),'ok'),hold on;
plot(x,y,'-r'),hold on;
title('真实轨迹');
legend('start','end');
figure(2)
plot(X(1,1),X(2,1),'og'),hold on;
plot(X(1,N),X(2,N),'ok'),hold on;
plot(X(1,:),X(2,:),'-b'),hold on;
title('二阶近似轨迹');
legend('start','end');
figure(3)
plot(1:N,abs(x-X(1,:)),'-m',1:N,abs(y-X(2,:)),'-y'),hold on;
title('近似误差');
legend('x-error','y-error');
%% ====================================================
%% EKF滤波程序
%% ====================================================
Z=[x;y];
Q=diag([0.08,0.06,0.01,0.01]); %一阶状态近似后误差
R=diag([0.5,0.4]);%直接观测x y 观测误差，实际很多观测传感器误差较大，此时误差模拟大于状态
H=[1 0 0 0;0 1 0 0];
randn('seed',0);
%% for t=2:N %对线性化状态加入随机噪声，卡尔曼迭代中并没有直接用，因为每次线性化迭代都进行了KF估计
%% X(:,t)=X(:,t-1)+sqrtm(Q)*randn(4,1); %加入噪声线性化的状态值
%% end
for t=1:N
Z(:,t)=Z(:,t)+sqrtm(R)*randn(2,1);%加入噪声的观测值
end
Xekf=zeros(4,N);
Xekf(:,1)=X(:,1);
Xnn(:,1)=X(:,1);%线性形似状态值
P0=eye(4);
for i=2:N
%%F矩阵计算%%%%%%%%%%%%%%
seita=w*i;
dx=-5*w*sin(seita);%一阶泰勒近似x=x+wdx 角度每次变化w
ddx=-5*w*cos(seita);%再次泰勒近似，雅克比矩阵计算
dxw=-5*sin(seita);
dy=5*w*cos(seita);
ddy=-5*w*sin(seita);
dyw=5*cos(seita);
F=[1 0 ddx dxw;0 1 ddy dyw;0 0 1 1;0 0 0 1];%雅克比矩阵
%%%%%%%%%%%%%%%%%%%%%%%%
Xn=F*Xekf(:,i-1);%状态迭代更新
Xnn(:,i)=Xn;%线性形似状态值
P1=F*P0*F'+Q ;%协方差更新
K=P1*H'*inv(H*P1*H'+R);%4*2%卡尔曼增益
Xekf(:,i)=Xn+K*(Z(:,i)-H*Xn);%滤波结果
P0=(eye(4)-K*H)*P1;%协方差迭代
end
figure(4)
hold on;box on;
plot(x,y,'-k.');%实际真实轨迹不可能知道，画图参考
plot(Z(1,:),Z(2,:),'-ob');
plot(Xnn(1,:),Xnn(2,:),'-g+');
plot(Xekf(1,:),Xekf(2,:),'-r+');
title('仿真轨迹');
legend('真实轨迹','观测轨迹','线性近似迭代状态轨迹','EKF轨迹');
figure(5)
hold on; box on;
plot(1:N,abs(x-Z(1,:)),'-r');
plot(1:N,abs(x-Xekf(1,:)),'-g');
title('仿真误差');
legend('观测 x-error','EKF x-error');