kalman.m

来自「基于T-s模糊模型用于估计非线性系统的状态的kalman滤波器。」· M 代码 · 共 31 行

function [xhat, P] = Kalman(xhat, y, sigmaX, sigmaY, P)

% function [xhat, P] = Kalman(xhat, y, sigmaX, sigmaY, P)
%
% Compute the state estimate.
% INPUTS
%   xhat = present state estimate
%   y = present measurement
%   sigmaX = standard deviation of process noise
%   sigmaY = standard deviation of measurement noise
%   P = state estimate covariance matrix
% OUTPUTS
%   xhat = updated state estimate
%   P = updated state estimate covariance

Sw = diag(sigmaX)^2;
Sv = diag(sigmaY)^2;

% Compute the T-S model system matrices and membership degrees.
[A1, A2, B1, B2, h1, h2] = FuzzyModel(xhat);

% Compute the global system matrices.
A = h1 * A1 + h2 * A2;
B = h1 * B1 + h2 * B2;

% Compute the present state estimate.
xhat = xhat + P * inv(P + Sv) * (y - xhat);

% Compute the state estimate covariance.
P = A * (P - P * inv(P + Sv) * P) * A' + Sw;