kalman.m

KALMAN FILTERING FOR FUZZY DYNAMIC SYSTEMS

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;