这个函数是一个离散多元卡尔曼滤波器对运动系统(系统建模,可以使用牛顿运动方程)和线性测量功能。
过滤功能,如估计过程噪声协方差和坏的测量数据检测。所以用户不需要知道噪声协方差矩阵(Q)。滤波器估计估计和更新这个矩阵使用先前“N”的状态估计的方差(屁股)。

坏的测量是基于确定的数据计算残差。如果剩余的绝对星等大于一个阈值集的标准偏差从预测方差(先验方差),然后测量数据被预测状态(前)。

%的例子:一只狗朝着单一的方向与常量。速度和可观察到的位置
%直接可测状态:x(位置)
%隐藏状态:x”(速度)

%差分方程:
% xk xk + 1 = + x ' _k * dt
% x ' _k + 1 = x ' _k +犯错(加速度恒定速度认为,犯错是错误的)
%状态转移矩阵(F) = = > [1 dt;0 1]
% [xk + 1;x ' _k + 1] = [1 dt;0 1]* [xk;x ' _k];

% %输出和输入

%的:问题的维度:在上面示例中,维度2(没有。的状态:位置和速度)

% updateState:更新后的状态(k_k + 1)

% updateStateVar:更新后的流程系统的协方差矩阵(P_k + 1)

% initStateBelieve:如初始假设状态。[1]的位置和速度,对pos(1 1 1),或者,加速度。

% initVarBelieve:初始假定方差的状态。[100 0;0 50]位置和速度的方差

% Q是过程方差,如果预先知道提到
如果未知% Q = []

% Calcuation的问
方差%这个问题,在这个过程中会形成错误的加速度(这个过程是一个恒定的速度)
%方差州[x;x ']由于accelation e = = > [dt ^ 2/2;dt] * velVar
%过程噪声Covariace Q = [dt ^ 2/2;dt] * [dt ^ 2/2 dt] * velVar = (0.25 * 0.5 dt ^ 4 * dt ^ 3; 0.5 * dt ^ 3 dt ^ 2] * velVar
%,velVar速度的方差和dt步伐。1、0.5、0.1(比如传感器的测量速度)
%引用书“评估与应用程序跟踪和导航”酒吧您好p.303

% z是测量向量或矩阵。
% H Measurement-State关系矩阵

% R测量误差协方差矩阵

% H = 0 [1];%测量功能,1位置和速度为0(我们不是衡量速度)

% badMeasCheck:‘上’检测不好测量,否则‘了’

%时间步是非常重要的,考虑不同效果的步伐问:较小的时间步长公式更好。