卡尔曼滤波（5）：一种用EKF解决问题的思路
这是书上给出的一个例子，我希望从中能归纳一种套路可以用在 大部分EKF问题中
一：建立数学模型
（一）：建立状态方程
状态方程是由具体问题的物理意义抽象出来的，不同问题具有不同的状态方程，本文为了说明问题使用的状态方程为
（二）：建立观测方程
观测方程也是由实际物理意义抽象出来的，本文使用的观测方程为
（三）：一阶线性化状态方程，求状态转移矩阵F(k)。（其实就是把状态方程求偏导的过程）
（四）：一阶线性化观测方程，求解观测矩阵H(k)
三四步是针对EKF才需要的步骤，除此之外有时还需要求对w和v的偏导，本文中由于例子简单，因此不需要进行
接下来进入卡尔曼滤波，首先是时间更新
（五）：状态预测
EKF的状态预测和KF的状态预测其实没有差别，都是假设没有过程噪声w，然后使用状态方程将上一时刻的后验估计值代入
本文中的X(k|k-1)是指用k-1时刻估计出来的k时刻的值，也即k时刻的先验估计值，同理X(k-1|k-1)就是指k-1时刻的后验估计值
（六）：观测预测
说实话当时看参考书这里一直没看懂，y不应该是传感器测出来的值吗，为什么要预测？不过后面用matlab进行仿真时要用到这个值，所以姑且把这个表达式保留下来。
（七）：求协方差预测
EKF的协方差预测和KF也是基本没差别，只不过是F（k）需要用偏导先求出来
接下来进入状态更新
（八）：求卡尔曼滤波增益
（九）：求状态后验估计值
其中是用观测方程假设观测误差为0计算出来的
（十）：协方差更新
至此，卡尔曼滤波需要的所有方程推导完毕，从（三）到（十）为一次EKF迭代
二：matlab仿真
接下来使用一个matlab程序进行仿真
（一）：确定需要的变量和数组
这是根据前面推导出的公式进行确定的
（1）：由（一）式，X要用到上一时刻的值，因此用数组，使用数组同时就确定了仿真次数N，当然如果是在实际工程中，一般是要使用无限死循环，那就不需要确定N了；
（2）：由（二）式，Y是测量值，在工程实际中使用变量就足够了，但是这里是仿真，最后要用Y值作图，所以这里也是用数组；
（3）：由（一、二）式，存在噪声误差，w,v方差矩阵分别为Q，R，为了简单起见，我们假设Q，R为常数，w,v则为数组；
（4）：由（三）式，F(k)只和上一时刻X的估计值有关，且每次迭代会更新，因此可以使用变量。在仿真中使用变量或者数组都可以，但是实际工程中往往需要用到嵌入式编程，这种情况下建议使用变量；
（5）：由（四）式，H（k），同上，数组变量均可，仿真建议用数组，嵌入式建议用变量；
（6）：由（五）式，X的先验估计量每一次迭代都需要重新计算，并且对于仿真没有影响，因此使用常数；
（7）：由（六）式，协方差矩阵的先验估计，同上，常数。由于先验估计需要使用上一时刻协方差矩阵的后验估计，因此后验估计使用数组；
（8）：由（八）式，卡尔曼增益K，每次迭代都需要重新计算，因此使用变量。
以上我们确定的在仿真程序中需要使用的变量和数组，现在开始写程序
（二）：matlab程序架构
这次是一个简单的仿真程序，因此架构十分简单，大致按照初始化，卡尔曼滤波，画图输出三部分设计
（三）：matlab仿真程序
%%%%%%%%%%%%%%%
%函数功能：标量非线性系统拓展卡尔曼滤波问题
%状态方程：X(k+1)=0.5X(k)+2.5X(k)/(1+X(k)^2)+8cos(1.2k)+w(k)
%观测方程：Z(k)=X(k)^2/20+v(k)
%好像不能用，我要花一定时间调试
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function EKV_For_One_Div_Unline_System
%初始化
T=50; %总时间
Q=10; %Q的值改变，观察不同Q值时滤波结果
R=1; %测量噪声
%产生过程噪声
w=sqrt(Q)*randn(1,T);
%产生观测噪声
v=sqrt(R)*randn(1,T);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%状态方程
x=zeros(1,T);
x(1)=0.1;
y=zeros(1,T);
y(1)=x(1)^2/20+v(1);
for k=2:T
x(k)=0.5*x(k-1)+2.5*x(k-1)/(1+x(k-1)^2)+8*cos(1.2*k)+w(k-1);
y(k)=x(k)^2/20+v(k);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%EKF滤波算法
Xekf=zeros(1,T);
Xekf(1)=x(1);
Yekf=zeros(1,T);
Yekf(1)=y(1);
P0=eye(1);
for k=2:T
%状态预测
Xn=0.5*Xekf(k-1)+2.5*Xekf(k-1)/(1+Xekf(k-1)^2)+8*cos(1.2*k);
%观测预测
Zn=Xn^2/20;
%求状态矩阵F
F=0.5+2.5*(1-Xn^2)/(1+Xn^2)^2;
%求观测矩阵
H=Xn/10;
%协方差预测
P=F*P0*F'+Q;
%求卡尔曼增益
K=P*H'*inv(H*P*H'+R);
%状态更新
Xekf(k)=Xn+K*(y(k)-Zn);
%协方差更新
P0=(eye(1)-K*H)*P;
end
%误差分析
Xstd=zeros(1,T);
for k=1:T
Xstd(k)=abs(Xekf(k)-x(k));
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%画图
figure
hold on; box on;
plot(x,'-ko','MarkerFace','g');
plot(Xekf,'-ks','MarkerFace','b');
legend('真实值','卡尔曼滤波估计值');
xlabel('时间/s');
ylabel('状态值x');
%误差分析
figure
hold on; box on;
plot(Xstd,'MarkerFace','g');
xlabel('时间/s');
ylabel('状态估计偏差');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%