卡尔曼滤波算法-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);