KF算法学习（二）：例程验证

最新推荐文章于 2024-07-10 11:08:56 发布

Mega_Li

最新推荐文章于 2024-07-10 11:08:56 发布

阅读量1.1k

点赞数

文章标签：算法

本文链接：https://blog.csdn.net/lwx309025167/article/details/78331417

版权

0前言

有关卡尔曼滤波算法的推导部分，见点击打开链接。这篇文章主要接着提供了一个matlab例程验证算法效果。

1.代码

代码中模拟的情况就是一辆从静止开始运动的小车，环境影响设置与点击打开链接中相同。通过对小车位置的观测值和预测值进行融合计算得到估计值。matlab代码如下

clc
clear all;
close all;

%初始化参数
delta_t = 0.1; %采样时间间隔
t = 0:delta_t:10;
N = length(t);
sz = [2,N];
g = 10; %加速度大小，对应卡尔曼滤波中的控制向量
q = 10; %系统位置噪声方差
Q = [q 0;0 0]; %系统噪声协方差矩阵，大小为2X2（系统状态中只包含位置、速度两个分量）
r = 5; %测量位置噪声方差
R = [r]; %测量噪声协方差，大小为1X1（只测量了位置）
A = [1 delta_t; 0 1]; %状态转换矩阵，大小为2X2
B = [1/2*delta_t^2; delta_t]; %控制转换矩阵，大小为2X1
H = [1,0]; %观测转换矩阵，大小为1X2

n = size(Q);
m = size(R);

P = zeros(n); %估计值和真实值的误差协方差矩阵
Qn = zeros(1,N); %系统噪声导致的小车位置偏差累积
Qn(1) = sqrt(q)*randn;
for i = 2:1:N
    Qn(i) = Qn(i-1)+sqrt(q)*randn; %每一个dt内位置噪声满足高斯分布
end

x = 1/2*g*t.^2 + Qn(int32(t/delta_t+1)); %真实值
z = x + sqrt(r).*randn(1,N); %加入观测噪声的观测值

xhat = zeros(sz); %估计值
xhatminus = zeros(sz); %预测值
Pminus = zeros(n); %预测值与真实值的误差协方差矩阵
K = zeros(n(1),m(1)); %卡尔曼增益矩阵
I = eye(n);

for k = 9:N %这里我们从时刻9处才开始计算，即小车已经运动了一段时间
    xhatminus(:,k) = A*xhat(:,k-1) + B*g; A*xhat(:,k-1)+B*g;
    Pminus = A*P*A'+Q;
    K = Pminus*H'/(H*Pminus*H'+R);
    xhat(:,k) = xhatminus(:,k)+K*(z(k)-H*xhatminus(:,k));
    P = (I-K*H)*Pminus;
end
 
figure(1)
plot(t,z) %观测值
hold on
plot(t,xhat(1,:),'r-') %估计值（卡尔曼滤波结果）
plot(t,xhatminus(1,:),'k-'); %预测值
hold on
plot(t,x(1,:),'g-') %真实值
legend('z','xhat','xhatminus','x');
xlabel('Iteration');