数学变换与滤波

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 % 一维仅高度，气压