一级倒立摆状态空间建模，离散化，Matlab仿真，添加高斯噪声并采用卡尔曼滤波去噪

锦瑞_

已于 2022-05-08 17:33:13 修改

阅读量4.7k

点赞数 47

文章标签： matlab

于 2022-05-08 17:09:48 首次发布

本文链接：https://blog.csdn.net/weixin_46143765/article/details/124650542

版权

最近做了一个和一级倒立摆有关的作业，做的时候在网上查阅了很多的资料，但是发现CSDN的很多资料文件都需要付费才能下载。本着开源的精神，搞一波分享，求点赞收藏。

代码直接放在开头，转载表明出处谢谢。当然，文中可能有很多错误，尤其是卡尔曼滤波部分，因为我对卡尔曼滤波一窍不通，只是任务需要去照猫画虎接进去罢了，仅供参考，轻喷。

1.先上代码，能跑才有下一步

（1）倒立摆建模部分：“Filter.m”

clc;
clear;

% 输入状态空间建模信息
A = [0 1 0 0;
    0 -0.0883 0.6293 0;
    0 0 0 1;
    0 -0.2357 27.8285 0];
B = [0;
    0.8832;
    0;
    2.3566];
C = [1 0 0 0;
    0 0 1 0];
D = 0;

% 模型离散化
T = 0.02;
[AA,BB] = c2d(A, B, T);

% 倒立摆引入LQR控制算法
Q = [1 0 0 0;
    0 1 0 0;
    0 0 2 0;
    0 0 0 2];
R = 1;
K = lqr(AA, BB, Q, R);
Ac = AA - BB*K;

% 确定模型初态、模型输入，并求解模型运动状态
x0=[0;
    2;
    0;
    1];
t = 0:0.01:9.99;
u = zeros(size(t));
[y, x] = lsim(Ac, BB, C, D, u, t, x0);
figure(1);
plot(t, y(:, 1), "b", "LineWidth", 1);
hold on;
plot(t, y(:, 2), "r", "LineWidth", 1);
legend('位移', '角度');
xlabel('时间/s');

figure(2);
% 提取位移信息
y_position = y(:,1);  
plot(t, y_position, "LineWidth", 1);
xlabel('时间/s');

% 假设位移信息读取受到严重噪声干扰，加入高斯噪声
Z = y_position;
for i = 1:length(Z)
	Z(i) = y_position(i) + normrnd(0,0.4);
end
figure(3);
plot(t, Z);
xlabel('时间/s');

% 以下是我一窍不通的卡尔曼滤波部分
r = 0.017;
D = Z-y_position;
X = Z(1);
T = 0.03;
V = 0.1;
Q = T^2*V^2;
R = r^2;
P = X^2;
A = 1;
H = 1;
Xn = KalmanFilter(X, Z, A, Q, H, R, P, y_position);

（2）卡尔曼滤波函数部分：“KalmanFilter.m”

function  Xkf = KalmanFilter(InitX, Z, F, Q, H, R, P0, y)

% 参数说明： InitX:初始值
%           Z:输入测量得到的数据
%           F：状态转移矩阵
%           H：观测矩阵
%           Q：协方差矩阵
%           R：测量协方差矩阵

X = InitX;
% 数据点数目
[l, s] = size(X);
L = length(Z);
% 初始化观测值矩阵 
Xkf = zeros(L,s);
Xkf(:,1) = X(:,1);
P = P0;
% 滤波 
for i = 2:L
    Xn = F*(Xkf(i-1,:)');
   P = F*P*F' + Q;
    K = P*H'*inv(H*P*H' + R);
    Xkf(i, :) = Xn + K*(Z(i,:) - H*Xn);
    P = (eye(s)-K*H)*P;
end 
fig = figure(10);
set(fig, 'position', [200 200 1200 500]);
hold on;
plot(Z(:,1),'-b.','MarkerSize',1);
hold on;
plot(Xkf(:,1),'-r','MarkerSize',1);
hold on;
plot(y(:,1),'-g','MarkerSize',1);
legend('带有噪声信号', '滤波后信号', '原信号');
xlabel('时间/ms');
end