卡尔曼滤波算法-MATLAB

by AI

classdef KalmanFilter < handle properties % 系统模型参数 A % 状态转移矩阵 B % 控制输入矩阵 H % 观测矩阵 Q % 过程噪声协方差 R % 测量噪声协方差 P % 状态估计协方差 x % 状态估计 end methods % 构造函数 function obj = KalmanFilter(A, B, H, Q, R, P, x0) obj.A = A; obj.B = B; obj.H = H; obj.Q = Q; obj.R = R; obj.P = P; obj.x = x0; end % 预测步骤 function predict(obj, u) % 预测状态 obj.x = obj.A * obj.x + obj.B * u; % 预测协方差 obj.P = obj.A * obj.P * obj.A' + obj.Q; end % 更新步骤 function update(obj, z) % 计算卡尔曼增益 K = obj.P * obj.H' / (obj.H * obj.P * obj.H' + obj.R); % 更新状态估计 obj.x = obj.x + K * (z - obj.H * obj.x); % 更新协方差 obj.P = (eye(size(obj.P)) - K * obj.H) * obj.P; end % 运行卡尔曼滤波 function run(obj, u, z) obj.predict(u);  obj.update(z);  end end 
end 

实例

% 系统模型参数 
A = [1 1; 0 1]; % 状态转移矩阵 
B = [0.5; 1];   % 控制输入矩阵 
H = [1 0];      % 观测矩阵 
Q = eye(2);     % 过程噪声协方差 
R = 1;          % 测量噪声协方差 
P = eye(2);     % 初始状态估计协方差 
x0 = [0; 0];    % 初始状态估计 % 创建卡尔曼滤波器对象 
kf = KalmanFilter(A, B, H, Q, R, P, x0); % 模拟数据 
u = 1; % 控制输入 
z = 2; % 测量数据 % 运行卡尔曼滤波 
kf.run(u,  z); % 输出状态估计 
disp(kf.x);