1、离散随机线性系统的卡尔曼滤波
在matlab中实现,代码如下:
% GPS精度为1m、气压计精度为0.5m,加速度计的精度为 1cm/s^2
% 无人机按照螺旋线飞行,半径为 20m,螺距为40m,100s完成一圈飞行
% 数据采集频率为 10Hz
clear all
D = 3; % 维度,可取 1,2,3
dt = 0.1; % 0.1s采集一次数据
t0 = 0:dt:100; % 0~100s
N = length(t0); % 采样点数
A = eye(D); % 状态转移矩阵,和上一时刻状态没有换算,故取 D阶单位矩阵
x = zeros(D, N); % 存储滤波后的数据
z = ones(D, N); % 存储滤波前的数据
x(:, 1) = ones(D,1); % 初始值设为 1(可为任意数)
P = eye(D); % 初始值为 1(可为非零任意数),取 D阶单位矩阵
r = 20; % 绕圈半径,20m
w = 2*pi / 100; % 计算出角速度,100s绕一圈
Q = 1e-2*eye(D); % 过程噪声协方差,估计一个
R = [1 0 0;
0 1 0;
0 0 0.5]; % 测量噪声协方差,精度为多少取多少
k = 1; % 采样点计数
if D==1 % 一维仅高度,气压