【EKF】卡尔曼滤波的二维应用实例

前言

在上期,使用一个简单的一维应用实例来加深了卡尔曼滤波的印象后,使用一个二维的例子来看一下卡尔曼的效果。使用一个自由落体的例子来说明,假设一个物体在重力作用下,速度由0开始做自由落体运动,有观测装置对该物体的位移进行观测,每 1 秒测量一次数据,规定向下为正,也就是说,加速度 a = g,状态方程的原始模型还是如下的形式:

X(k) = A*X(k-1) + B*u(k-1) + w(k-1)
Z(k) = H*X(k) + v(k)
其中 w 为过程噪声,方差为Q, v 为测量噪声,方差为R

状态方程的建立

根据自由落体的公式,可以建立如下的方程:

速度 v = a*t

位移 s = a*t*t / 2

写成离散的形式为:

速度 v(k) = v(k-1) + a * dt

位移 s(k) = s(k-1) + [v(k-1) + v(k)] * dt / 2

由于 v(k) = v(k-1) + a * dt,因此 s(k) = s(k-1) + v(k-1) * dt + a*dt/2

为了方便计算,我们将 dt 设置为1,也就是 1 秒作为时间间隔进行观测与估算,同时,a = g,那么上式就会简化为:

v(k) = v(k-1) + g

s(k) = s(k-1) + v(k-1) + 0.5 * g

将上式写作状态方程的形式,定义 x1 代表位移,x2 代表速度,x1_dot = x2 (x1的微分就是x2),将x1,x2合并为一个二维的状态矩阵 x

则离散形式的状态方程为:

测量方程为:

z(k) = [1 0]x(k) + r(k)

其中,r 为测量噪声,传感器只能观测位移,不能观测速度

matlab程序编写

同上篇文章的一维应用一样,对系统各个参数先进行赋值,按照上面建模的模型数据进行赋值即可,需要注意的是 Q 矩阵,赋值为 x1 的方差与 x2 的方差:

N = 10;   %采样个数  每1s采样一次,采样100s
temp_real = 0;   %实际距离
%系统状态方程为:
%   X(k) = A*X(k-1) + B*u(k-1) + w(k-1)
%   Z(k) = H*X(k) + v(k)
%   其中 w 为过程噪声,方差为Q,  v 为测量噪声,方差为R
%   u输入为重力加速度 g
A = [1 1; 0 1];  %
B = [0.5;1];
u = 9.8;  %输入为重力加速度
H = [1 0];  %由于是一维系统,因此H矩阵为1
Q = [1^2 0; 0 1^2];  %假定过程噪声的方差为1*1  即加入空气阻力等因素的影响造成的系统建模误差
R = 10.0^2;  %假定测量噪声的方差为1.0*1.0
x_real = temp_real*ones(2,N);   %将实际值赋值二维容器中
x_hat = zeros(2,N);  %先验估计值  二维
xhat = zeros(2,N);  %估计值
z = zeros(1,N);  %位移传感器测量值
P = Q;  %协方差矩阵 初始为Q
P_ = P; %先验协方差矩阵
z(1) = 0;  %初始位移
x_hat(1) = z(1);  %初始估计值
w = sqrt(Q)*randn(2,N);   %过程噪声   方差的开方就是噪声的大小
v = sqrt(R)*randn(1,N);   %测量噪声   方差的开方就是噪声的大小

之后,按照卡尔曼滤波的五大公式,进行数据更新:

【EKF】卡尔曼滤波的二维应用实例_第1张图片

在编程时,需要注意一点,在计算卡尔曼增益时,上篇的一维系统,由于矩阵都是一维,因此是可以直接使用 '/' 除号的,但矩阵非一维后,需要使用 inv 来求矩阵的逆来代替除号。

除此之外,在更新实际值时,让真实位移再增加一个在方差范围内的误差,模拟空气阻力等因素的影响:

for k = 2:N
    %修改实际值,让真实温度每次递增 但增量需要在方差以内
    x_real(:,k) = A*x_real(:,k-1) + B * u + w(:,k);
    %获取测量值
    z(k) = H * x_real(:,k) + v(k);  %加入测量噪声
    %计算先验估计值
    x_hat(:,k) = A * xhat(:,k-1);  
    %计算先验协方差
    P_ = A * P * A' + Q;
    %计算卡尔曼增益  由于是二维矩阵,因此不能直接使用/除号,需要使用inv来求逆
    K = (P_ * H') * inv(H * P_ * H' + R);
    %计算后验估计值
    xhat(:,k) = x_hat(:,k) + K * (z(k) - H * x_hat(:,k));
    %更新估计误差协方差
    P = (1 - K * H) * P_;
end

画出数据的对比图:

【EKF】卡尔曼滤波的二维应用实例_第2张图片

你可能感兴趣的:(EKF,卡尔曼滤波,EKF)