最近学习卡尔曼滤波器，书写过程老是出错，问题有flag=1,flag=3的最多，基本解决方法就是要保证维度要正确（尤其是当输入的参数没有直接作为输入向量中，例只用了u(1)，u(2)，而剩下的u(3)……都没有作为输入变量，因此在后面的公式中需要将原公式*u变成*[u(1);u(2)]，这点是最容易忽略的地方。还有就是在初始值设计的时候，要注意状态变量的个数，并把数目写到离散状态变量或者连续状态变量上。最后的一点是在计算中需要保证没有inf，和nan出现。一般这种出现了除号，并且除零，所以要避免这种事的发生。）

卡尔曼滤波

卡尔曼滤波是隐马尔科夫的一个应用。通俗来讲，就是此时状态量只和上一时刻的状态量有关，观测也只依赖状态量。

在使用两个传感器时，有误差就可以通过取平均值（不准确），之后变成了取加权平均值。加权平均值的存在在实际中还是一个概率问题。而测量的传感器都是服从正态分布的，那么就有方差和均值。那有了这些数据就能得到一个较为准确的测量值。但是通常只有一个传感器。那么这个时候就要通过建立一个数学模型来代替另一个传感器的工作。数学模型是如何得到一个估计的测量值呢？

卡尔曼滤波器的作用就是将通过观测值和已知数据估计状态变量。simulink中状态空间模块是输入已知数据，得到输出数据，而里面的状态量x不知道，而虽然可以让C矩阵变成一个单位矩阵来得到状态向量，但是面对拥有噪声的输入源，是没办法得到准确的数据。应用中，卡尔曼用于滤波，设计中就是通过状态方程和观测方程，已知状态方程的输入和观测方程的输出来推算状态方程。

首先先用输入向量（以及初始设置的状态向量的值）通过状态方程得到状态向量的估计值，在通过量测方程代入估计值算出观测值的估计值，再算出一个增益（就是看这个估计值可信程度），和实际输入的观测值进行比较，将增益和状态向量的估计值通过公式，得到下一个状态向量的估计值。到此一个循环结束。

在下列代码中，如果代入公式去算，发现矩阵ABCD 的取值是不合理的。仿真出结果后，发现输入的观测值和估计出来的观测值完全不一样，并且差距很大。让我一度怀疑是不是估计出了问题，这样也没办法验证状态值的估计是否是正确的。然后我通过加一个离散状态空间模块来对这个进行反向模拟，有同样的输入，前提（ABCD矩阵大小一样），得到了在状态方程的状态向量和观测向量的输出。发现主要问题就是观测值是随便给的值，因此估计出的值是正确的，将估计的值给到真实观测值作为输入，整个估计就显示的比较合理了。

以下就是我在MATLAB论坛上找到的卡尔曼滤波的S函数进行更改后的（原文章使用的是1*1，我改的是2*1）

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % S函数实现对输入信号Kalman滤波 function [sys,x0,str,ts] = SimuKalmanFilter1(t,x,u,flag) % 输入参数： % t, x, u分别对应时间、状态、输入信号 % flag为标志位，其取值不同，S函数置信的任务和返回的数据也是不同的 % 输出参数： % sys为一个通用的返回参数值，其数值根据flag的不同而不同 % x0为状态的初始数值 % str为目前为止的matlab版本中并没有什么作用，一般str=[]即可 % ts为一个两列的矩阵，包含采样时间和偏移量两个参数V

switch flag  case 0 % 系统进行初始化，调用mdlInitializeSizes函数              [sys, x0, str, ts]=mdlInitializeSizes;  case 1 % 计算连续状态变量的导数，调用mdlDerivatives函数              sys=mdlDerivatives(t, x, u);  case 2 % 更新离散状态变量，调用mdlUpdate函数              sys=mdlUpdate(t, x, u);  case 3 % 计算S函数的输出，调用mdlOutputs函数              sys=mdlOutputs(t, x, u);  case 4 % 计算下一仿真时刻              sys=mdlGetTimeOfNextVarHit(t, x, u);  case 9 %仿真结束，调用mdlTerminate函数              sys=mdlTerminate(t, x, u);  otherwise %其他未知情况处理，用户可以自定义             error(['Unhandled flag=',num2str(flag)]); end

% 1. 系统初始化子函数 function [sys, x0, str, ts]=mdlInitializeSizes sizes=simsizes; sizes.NumContStates=0; sizes.NumDiscStates=2; sizes.NumOutputs=2; sizes.NumInputs=1; sizes.DirFeedthrough=1; sizes.NumSampleTimes=1; %至少需要的采样时间 sys=simsizes(sizes); x0=[0;0]; %初始条件 str=[]; %str总是设置为空 ts=[-1 0]; % 表示该模块采样时间继承其前的模块采样时间设置 global P; %定义协方差矩阵 P=[1 0;0 1];

% end mdlOutputs % ========================================================================

% 2. 进行连续状态变量的更新 function sys=mdlDerivatives(t, x, u) sys=[];

% end mdlDerivatives % ========================================================================

% 3. 进行离散状态变量的更新 function sys=mdlUpdate(t, x, u) global P; F=[1 0.1;0 1]; B=[0;0]; H=[1 0]; Q=[0.0001 0;0 0.0001]; R=1; xpre=F*x+B*u; % 状态预测 Ppre=F*P*F'+Q; % 协方差预测 K=Ppre*H'*inv(H*Ppre*H'+R); % 计算Kalman增益 e=u-H*xpre; %u是输入的观测值，在此计算新息 xnew=xpre+K*e; %状态更新 P=(eye(2)-K*H)*Ppre; %协方差更新 % 将计算的结果返回给主函数 xnew1=xnew(1,1); xnew2=xnew(2,1); sys(1)=xnew1; sys(2)=xnew2;

% end mdlUpdate % ========================================================================

% 4. 求取系统的输出信号 function sys=mdlOutputs(t, x, u) sys(1)=x(1); sys(2)=x(2); %把算得的模块输出向量赋给sys

% end mdlOutputs % ========================================================================

% 5. 计算下一仿真时刻，由sys返回 function sys=mdlGetTimeOfNextVarHit(t, x, u) sampleTime=1; %在此设置下一仿真时刻为1s以后 sys=t+sampleTime;

% end mdlGetTimeOfNextVarHit % ========================================================================

% 6. 结束仿真子函数 function sys=mdlTerminate(t, x, u) sys=[];

% end mdlTerminate % ========================================================================

同我的文章基础1，我还是给出外部输入参数的例子

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % S函数实现对输入信号Kalman滤波 function [sys,x0,str,ts] = SimuKalmanFilter2(t,x,u,flag) % 输入参数： % t, x, u分别对应时间、状态、输入信号 % flag为标志位，其取值不同，S函数置信的任务和返回的数据也是不同的 % 输出参数： % sys为一个通用的返回参数值，其数值根据flag的不同而不同 % x0为状态的初始数值 % str为目前为止的matlab版本中并没有什么作用，一般str=[]即可 % ts为一个两列的矩阵，包含采样时间和偏移量两个参数V

switch flag  case 0 % 系统进行初始化，调用mdlInitializeSizes函数              [sys, x0, str, ts]=mdlInitializeSizes;  case 1 % 计算连续状态变量的导数，调用mdlDerivatives函数              sys=mdlDerivatives(t, x, u);  case 2 % 更新离散状态变量，调用mdlUpdate函数              sys=mdlUpdate(t, x, u);  case 3 % 计算S函数的输出，调用mdlOutputs函数              sys=mdlOutputs(t, x, u);  case 4 % 计算下一仿真时刻              sys=mdlGetTimeOfNextVarHit(t, x, u);  case 9 %仿真结束，调用mdlTerminate函数              sys=mdlTerminate(t, x, u);  otherwise %其他未知情况处理，用户可以自定义             error(['Unhandled flag=',num2str(flag)]); end

% 1. 系统初始化子函数 function [sys, x0, str, ts]=mdlInitializeSizes sizes=simsizes; sizes.NumContStates=0; sizes.NumDiscStates=2; sizes.NumOutputs=2; sizes.NumInputs=3; sizes.DirFeedthrough=1; sizes.NumSampleTimes=1; %至少需要的采样时间 sys=simsizes(sizes); x0=[0;0]; %初始条件 str=[]; %str总是设置为空 ts=[-1 0]; % 表示该模块采样时间继承其前的模块采样时间设置 global P; %定义协方差矩阵 P=[1 0;0 1];

% end mdlOutputs % ========================================================================

% 2. 进行连续状态变量的更新 function sys=mdlDerivatives(t, x, u) sys=[];

% end mdlDerivatives % ========================================================================

% 3. 进行离散状态变量的更新 function sys=mdlUpdate(t, x, u) global P; m=u(2); w=u(3); F=[m w;0 1]; B=[0;0]; H=[1 0]; Q=[0.0001 0;0 0.0001]; R=1; xpre=F*x+B*u(1); % 状态预测 Ppre=F*P*F'+Q; % 协方差预测 K=Ppre*H'*inv(H*Ppre*H'+R); % 计算Kalman增益 e=u(1)-H*xpre; %u是输入的观测值，在此计算新息 xnew=xpre+K*e; %状态更新 P=(eye(2)-K*H)*Ppre; %协方差更新 % 将计算的结果返回给主函数 xnew1=xnew(1,1); xnew2=xnew(2,1); sys(1)=xnew1; sys(2)=xnew2;

% end mdlUpdate % ========================================================================

% 4. 求取系统的输出信号 function sys=mdlOutputs(t, x, u) sys(1)=x(1); sys(2)=x(2); %把算得的模块输出向量赋给sys

% end mdlOutputs % ========================================================================

% 5. 计算下一仿真时刻，由sys返回 function sys=mdlGetTimeOfNextVarHit(t, x, u) sampleTime=1; %在此设置下一仿真时刻为1s以后 sys=t+sampleTime;

% end mdlGetTimeOfNextVarHit % ========================================================================

% 6. 结束仿真子函数 function sys=mdlTerminate(t, x, u) sys=[];

% end mdlTerminate % ========================================================================

在书写类似的函数的时候，注意U的输入值很多，在公式中使用到的有哪些，理清每一个参数的矩阵大小，出问题了就按最开始的那些方向开始检查就行。