数学变换与滤波
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 % 一维仅高度，气压计数据
true1D = t0*0.4;
elseif D==2 % 二维 x,y 方向，GPS数据
true2D = [r*cos(w*t0); r*sin(w*t0)];
elseif D==3 % 三维 x,y,z方向，GPS和气压计
true3D = [r * cos(w*t0); r * sin(w*t0); t0 * 0.4];
end
for t = dt:dt:100
k = k + 1;
x(:,k) = A * x(:,k-1); % 卡尔曼公式1
P = A * P * A' + Q; % 卡尔曼公式2
H = eye(D);
K = P*H' * inv(H*P*H' + R); % 卡尔曼公式3
if D==1 % 一维仅高度（z方向）
z(:,k) = true1D(k) + randn;
elseif D==2 % 二维 x,y 方向
z(:,k) = [true2D(1,k) + randn; true2D(2,k) + randn];
elseif D==3 % 三维 x,y,z 方向
z(:,k) = [true3D(1,k) + randn; true3D(2,k) + randn; true3D(3,k) + randn];
end
x(:,k) = x(:,k) + K * (z(:,k)-H*x(:,k)); % 卡尔曼公式4
P = (eye(D)-K*H) * P; % 卡尔曼公式5
end
if D==1 %% 一维情况
plot(t0, z,'b.'); % 绘制滤波前数据
axis('equal');hold on;grid on; % 坐标等距、继续绘图、添加网格
plot(t0, x,'r.'); % 绘制滤波后数据
plot(t0, true1D ,'k-'); % 绘制真实值
legend('滤波前','滤波后','理想值'); % 标注
xlabel('时间: s');
ylabel('高度: m');hold off;
elseif D==2 %% 二维情况
plot(z(1,:),z(2,:),'b.'); % 绘制滤波前数据
axis('equal');grid on;hold on; % 坐标等距、继续绘图、添加网格
plot(x(1,:),x(2,:),'r.'); % 绘制滤波后数据
plot(true2D(1,:), true2D(2,:), 'k.'); % 绘制真实值
legend('滤波前','滤波后','理想值');
xlabel('x方向: m');
ylabel('y方向: m');hold off;
elseif D==3 %% 三维情况
plot3(z(1,:),z(2,:),z(3,:),'b.'); % 绘制滤波前数据
axis('equal');grid on;hold on % 坐标等距、继续绘图、添加网格
plot3(x(1,:),x(2,:),x(3,:),'r.'); % 绘制滤波后数据
plot3(true3D(1,:), true3D(2,:), true3D(3,:));% 绘制滤波后数据
legend('滤波前','滤波后','理想值'); % 绘制真实值
xlabel('x方向: m');
ylabel('y方向: m');
zlabel('高度: m');hold off;
end
结果如图所示：
2、gamma---beta---伽马滤波
在matlab中实现，代码如下：
%% 参数设置
N = 200; % 设置数据长度为N
t = (1:N); % 生成时间轴
a = 1; % 状态转移方程
b = 0; % 控制输入
c = 1; % c: 观测方程
x = 5; % 设置初值 初始的均值和方差
sigma2 = 10;
V = 50; % 设置生成的信号的噪声标准差
Q = 1; % 设置状态转移方差Q和观测方差R
R = 50;
%% 初始化
real_signal = zeros(1,N); % 真实信号
z = zeros(1,N); % 观测数据
x_filter = zeros(1,N); % 存储卡尔曼估计的结果，缓存起来用于画图形
K = zeros(1,N); % 存储卡尔曼预测中的增益k，缓存起来用于画图形
% 初始化真实数据和观测数据
for i=1:N
%生成真实数据，为1-100线性增加的信号
real_signal(i) = i*2;
%real_signal(i) = 200;
%生成观测，真实数据基础上叠加一个高斯噪声 normrnd(均值, 标准差)
z(i) = real_signal(i) + normrnd(0, V);
end
%% 卡尔曼滤波
for i=1:N
% 预测步
x_ = a*x + b; %预测当前状态
sigma2_ = a*sigma2*a'+Q;
% 更新步
k = sigma2_*c'/(c*sigma2_*c'+R);
x = x_ + k*(z(i) - c*x_);
sigma2 = (1-k*c)*sigma2_;
% 存储滤波结果
x_filter(i) = x;
K(i) = k;
end
%% 展示
% 画出卡尔曼增益k 可以看到系统很快会达到稳态，k会很快收敛成为一个常数，此时卡尔曼滤波也就相当于低通滤波了
plot(t, K);legend('K');
% 画出波形， 真实值 - 观测值 - 卡尔曼估计值
figure(2)
plot(t, real_signal, 'r', t, z, 'g', t, x_filter, 'b')
legend('real','measure','filter');
结果如图所示
