卡尔曼滤波的matlab实现

任逸昂

宠辱不惊，看庭前花开花落；去留无意，望天上云卷云舒

我们这里假设读者已经完全掌握了matlab的理论推导，想要直接上手，用matlab进行实践。

笔者是读这篇文章入门的：

想要简单了解卡尔曼滤波的话，可以直接看维基百科对其的介绍；想要具体了解其所有背景和详细推导的话，推荐读《概率机器人》的前32页，曹玉红译，机械工业出版社。

话不多说，先上代码：

function [P, X_hat] = Kalman_filter(N, T, X, Y, A, C, Q, R) % 卡尔曼滤波模块 % 输入传感器维度N 数据长度T 矩阵真实值X 观测值Y 对应系数矩阵A 和 C 对应协方差系数Q 和R % 输出矩阵P和X_hat X_hat = ones(N, T); P = ones(N, N, T); % 初始化P矩阵, X-hat矩阵 for i=1:T-1 % 预测步 x_old = X(:, i); P_old = A * P(:, :, i) * A' + Q; % 更新步 K_new = (P_old * C') * inv(C * P_old * C' + R); x_new = x_old + K_new * (Y(:, i) - C * x_old); P_new = (eye(N, N) - K_new * C) * P_old; % 放到矩阵里面去 X_hat(:, i+1) = x_new; P(:, :, i+1) = P_new; end

以上为matlab关于卡尔曼滤波的自定义函数，以下做一个简单的展示：

clc;clear; rng('default') % For reproducibility 生成数据部分 N = 2; % 假设传感器维度是2 T= 10; % 假设全体时间是10 X = zeros(N, T); X(:, 1) = ones(size(X(:, 1))); % 初始化x_ik矩阵且定义好它的维度 A = ones(N,N) * 1.2; % 给出A矩阵 % 假设状态转移矩阵固定 Q = eye(2); % 给定误差阵Q mu = zeros(N, 1); omega = mvnrnd(mu, Q, T)'; % 基于误差阵Q生成omega_ik Y = zeros(N, T); Y(:, 1) = ones(size(Y(:, 1))) * 2; % 初始化y_ik矩阵且定义好它的维度 C = ones(N,N) * 1.3; % 给出C矩阵 % 假设状态转移矩阵固定 R = eye(2); % 给定误差阵Q mu = zeros(N, 1); nu = mvnrnd(mu, R, T)'; % 基于误差阵R生成nu_ik for i = 1:T-1 X(:, i+1) = A * X(:, i) + omega(:, i); Y(:, i+1) = C * Y(:, i) + nu(:, i); % 计算所有的x和y的数值卡尔曼滤波部分 [P, X_hat] = Kalman_filter(N, T, X, Y, A, C, Q, R); % 带入卡尔曼滤波函数