实际工程项目中是怎么用卡尔曼滤波的?
回答一
链接:https://www.zhihu.com/question/358334095/answer/1160183841
两大难题,一是运动学模型的建立,也就是预测方程。这个一方面可以通过比较细的系统辨识来做,这方面要结合具体的应用背景,不一而足,也是普遍做的比较差的地方。
另一方面可用某个传感器的输出来做预测,比较典型的就是用加速度计做预测,再用陀螺做更新。
另一大问题就是噪声的统计特性咯。因为建模误差不知道。观测方程到还可以测一下ZRO算一下均值和方差。然后下一步就是大家喜闻乐见的调参的阶段咯,据说是群魔乱舞花样百出。
我现实中只用过mems加速度计和陀螺的融合,还有就是无模型的跟踪咯。也就是预测方程是0,纯靠建模误差的那个方差来做预测,效果咋说呢,稳态下看噪声统计特性是好了些,但是动态特性明显滞后和变形了。应该是我经验不足吧
回答二
链接:https://www.zhihu.com/question/358334095/answer/1009240518
先啰嗦一下在工程中卡尔曼滤波要解决的问题是什么。
我们得到了2(或多个)个反馈同一件事的信息A和B,但不知道该相信哪个结果,所以简单的,将两个信息相加除以2(加权平均),得到一个结果0.5A+0.5B。但这个方法过于粗暴,不能反映出水平。现在知道了其中一个信息可靠度更好,而另一个可靠度不好,就要更相信好的信息,如,现在给可信度好的B更大的权重,得到0.2A+0.8B,这个结果会好于第一个。但仍然觉得low,因为A与B的参数是根据经验设置的,并不知道是否是最合理的,是否是最优化的。所以高级的,用优化的方法,根据A与B的高斯分布特征,去融合A和B信息,这个就是卡尔曼。
所以,我们看到,要利用卡尔曼,需要不同的信息源(大于等于2个)。最常见的信息源是什么?就是我们根据系统模型或动力学模型计算出来的结果,可以通过仿真的形式得到。第二个或第三个信息是什么?在飞行器姿态估计,导航定位,目标跟踪(几个问题的本质一样,都是状态估计)的问题中,这个信息一般来自惯导或GPS或雷达等。有了2个信息数据,就可以根据Kalman的公式进行融合。下面举一个机器人运动估计的例子,来自《Probabilistic Robotics》。
首先假设你的机器人长这样(一个圆,比较简单),在二维笛卡尔坐标系中运动。
根据这个图,可以得到它的运动学模型
经典的速度、角速度、角度模型,飞行器、轮式机器人、AUV都可以这样简单描述
得到以上模型还不能直接用,这个模型有变量的乘积和三角函数,是个非线性模型,但要利用卡尔曼公式,需要满足EKF(实际工程应用中,我还没见过直接用KF的)的线性化条件。所以要将非线性的模型线性化。
到这一步,得到的Gt可以直接代入EKF公式去更新协方差矩阵了,经过推算会得到增益矩阵K。EKF公式包括预测与更新,这里就不啰嗦了。
同时,用状态的迭代形式,可以得到初步推算的位置。
最后,EKF公式中有一项
(z是最终的观测值,最简单的,观测矩阵可以是单位阵,反应的是全状态)
其中两个z,代表了2个信息,比如其中一个是通过动力学得到的机器人位置,另一个通过雷达得到的机器人位置。
通过以上的计算,会得到一个靠谱的结果。
Kalman的其它公式都是用优化理论去解算高斯函数的,在此不啰嗦了。
回答三
链接:https://www.zhihu.com/question/358334095/answer/2396600132
关于卡尔曼原理的讲解知乎上有太多了,而且理解的都比我好,在这里我就不班门弄斧了。我实现了一个融合惯导的加速度计和陀螺仪的简易卡尔曼滤波器,代码写的十分简单,保证初学者一看就懂,对那些复现算法有困难的同学会起到一定帮助。话不多说,上干货: 会用到的卡尔曼脑仁疼公式如下:
加速度计数学模型满足:
代码融合如下,代码风格不好,各位别学我。(写这个代码的时候我还未成年):
#include "imu.h"
#include <iostream>
#include<Eigen/Core>
#include<Eigen/Dense>
#include<string>
#include<vector>
#include <cmath>
#include <limits>//用于生成随机分布数列
#include <stdlib.h>
#include <random>
#include <fstream>
#include <cstdlib> // 标准库
using namespace std;
using namespace Eigen;
using Eigen::MatrixXd;
int t=1;
double theta_PI=57.29578;
/* 定义系数*/
MatrixXd A(2,2);
MatrixXd B(2,1);
MatrixXd Qk(2,2);
MatrixXd H(1,2);
MatrixXd R(1,1);
MatrixXd I=MatrixXd::Identity(2,2);
Eigen::MatrixXd angle_1(1,1);vector<MatrixXd> X_k;MatrixXd Xk=MatrixXd::Constant(2,1,0);
vector<MatrixXd> X_k_1;MatrixXd Xk_1=MatrixXd::Constant(2,1,0);vector<MatrixXd> P_k;MatrixXd Pk=MatrixXd::Constant(2,2,0);
vector<MatrixXd> P_k_1;MatrixXd Pk_1=MatrixXd::Constant(2,2,0);
vector<MatrixXd> K_K;MatrixXd K=MatrixXd::Constant(2,2,0);
vector<MatrixXd> Y_K;MatrixXd Yk=MatrixXd::Constant(1,1,0);class Kalman
{public: Kalman()
{
IMU imu;
//A<<1,imu.dt,0,1;
//B<<imu.dt,0;
Qk<<0.03,0,0,0.003;//QK的值暂定一个常量,后续需要求解角度方差和角速度偏差方差
H(0,0)=1;H(0,1)=0;
R<<3;//测量噪声,暂时给一个定值 X_k.push_back(Xk);
X_k_1.push_back(Xk_1);
P_k.push_back(Pk);//此时刻的PK
P_k_1.push_back(Pk_1);//上一时刻的PK
K_K.push_back(K);
Y_K.push_back(Yk); imu_sub=nd.subscribe<sensor_msgs::Imu>("imu", 100, &Kalman::IMUHandle,this);
position_pub=nd.advertise<sensor_msgs::Imu>("imu_position",1000);
} void IMUHandle(const sensor_msgs::Imu::ConstPtr &imu_measure)
{
IMU Tx;
Tx.angle_v_x=imu_measure->angular_velocity.x;
Tx.angle_v_y=imu_measure->angular_velocity.y;
Tx.angle_v_z=imu_measure->angular_velocity.z; Tx.acc_x=imu_measure->linear_acceleration.x;
Tx.acc_y=imu_measure->linear_acceleration.y;
Tx.acc_z=imu_measure->linear_acceleration.z;
double angle=-atan2(Tx.acc_x,sqrt(Tx.acc_y*Tx.acc_y+Tx.acc_z*Tx.acc_z)); double angle_1=angle*theta_PI; double angle_v=Tx.angle_v_x; // cout<<Tx.acc_x<<" "<<Tx.acc_y<<" "<<Tx.acc_z<<" "<<angle_1<<endl;
kalman_angle_x(angle,angle_v); } void kalman_angle_x(double angle,double angle_V)//angle是加速度计算出的角度 ,angle_V 角速度
{ IMU imu;
angle_1(0,0)=angle;
//卡尔曼第1个公式
Xk_1(0,0)=(angle_V-imu.bias)*(t-1)*0.04;
Xk_1(1,0)=imu.bias;
X_k_1[0](0,0)=Xk_1(0,0);
X_k_1[0](1,0)=Xk_1(1,0); A<<1,imu.dt*t,0,1;
B<<imu.dt*t,0;
Xk=A*X_k_1[t-1]+B*angle_V;
X_k.push_back(Xk);//卡尔曼第2个公式
Pk=A*P_k_1[t-1]*A.transpose()+Qk;
P_k.push_back(Pk);//卡尔曼第3个公式
MatrixXd tmp(1,1);
tmp=H*P_k[t]*H.transpose()+R;
K=P_k[t]*H.transpose()*tmp.inverse();
K_K.push_back(K); Yk=angle_1-H*X_k[t];
Y_K.push_back(Yk);//卡尔曼第4个公式
Xk_1=X_k[t]+K_K[t]*Y_K[t];
X_k_1.push_back(Xk_1);
//卡尔曼第5个公式
Pk=(I-K_K[t]*H)*P_k[t];
P_k_1.push_back(Pk);
cout<<X_k[t](0,0)-angle<<endl;
// cout<<angle<<endl;
t++; }private:
ros::NodeHandle nd;
ros::Publisher position_pub;
ros::Subscriber imu_sub;};int main(int argc, char** argv)
{
ros::init(argc, argv,"kalman"); Kalman PublicIMU; ros::spin();
return 0;
}
完整的代码在我的gitee:
https://gitee.com/angry-potato/imu-kalman
效果展示在这里:
纵坐标是观测值和预测值之间的error,数据输入全部都是来源于惯导,当我快速抖动惯导之后,就有了上面的波浪部分,但是很快error又趋近于0。
什么?????你说你看不懂C++?
我这里也有份MATLAB的代码,可以参考一下,不过是网上的:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%程序功能: 使用扩展卡尔曼滤波器(EKF)估计平抛物体的运动
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc;
clear;
close all;
kx = .01; ky = .05; % 阻尼系数
g = 9.8; % 重力
t = 10; % 仿真时间
Ts = 0.1; % 采样周期
len = fix(t/Ts); % 仿真步数
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%(真实轨迹模拟)
dax = 1.5; day = 1.5; % 系统噪声
X = zeros(len,4); X(1,:) = [0, 50, 500, 0]; % 状态模拟的初值
for k=2:len
x = X(k-1,1); vx = X(k-1,2); y = X(k-1,3); vy = X(k-1,4);
x = x + vx*Ts;
vx = vx + (-kx*vx^2+dax*randn(1,1))*Ts;
y = y + vy*Ts;
vy = vy + (ky*vy^2-g+day*randn(1))*Ts;
X(k,:) = [x, vx, y, vy];
end
figure(1), hold off, plot(X(:,1),X(:,3),'-b'), grid on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 构造量测量
mrad = 0.001;
dr = 10; dafa = 10*mrad; % 量测噪声
for k=1:len
r = sqrt(X(k,1)^2+X(k,3)^2) + dr*randn(1,1);
a = atan(X(k,1)/X(k,3)) + dafa*randn(1,1);
Z(k,:) = [r, a];
end
figure(1), hold on, plot(Z(:,1).*sin(Z(:,2)), Z(:,1).*cos(Z(:,2)),'*')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ekf 滤波
Qk = diag([0; dax; 0; day])^2;
Rk = diag([dr; dafa])^2;
Xk = zeros(4,1);
Pk = 100*eye(4);
X_est = X;
for k=1:len
Ft = JacobianF(X(k,:), kx, ky, g);
Hk = JacobianH(X(k,:));
fX = fff(X(k,:), kx, ky, g, Ts);
hfX = hhh(fX, Ts);
[Xk, Pk, Kk] = ekf(eye(4)+Ft*Ts, Qk, fX, Pk, Hk, Rk, Z(k,:)'-hfX);
X_est(k,:) = Xk';
end
figure(1), plot(X_est(:,1),X_est(:,3), '+r')
xlabel('X'); ylabel('Y'); title('ekf simulation');
legend('real', 'measurement', 'ekf estimated');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function F = JacobianF(X, kx, ky, g) % 系统状态雅可比函数
vx = X(2); vy = X(4);
F = zeros(4,4);
F(1,2) = 1;
F(2,2) = -2*kx*vx;
F(3,4) = 1;
F(4,4) = 2*ky*vy;
end
function H = JacobianH(X) % 量测雅可比函数
x = X(1); y = X(3);
H = zeros(2,4);
r = sqrt(x^2+y^2);
H(1,1) = 1/r; H(1,3) = 1/r;
xy2 = 1+(x/y)^2;
H(2,1) = 1/xy2*1/y; H(2,3) = 1/xy2*x*(-1/y^2);
end
function fX = fff(X, kx, ky, g, Ts) % 系统状态非线性函数
x = X(1); vx = X(2); y = X(3); vy = X(4);
x1 = x + vx*Ts;
vx1 = vx + (-kx*vx^2)*Ts;
y1 = y + vy*Ts;
vy1 = vy + (ky*vy^2-g)*Ts;
fX = [x1; vx1; y1; vy1];
end
function hfX = hhh(fX, Ts) % 量测非线性函数
x = fX(1); y = fX(3);
r = sqrt(x^2+y^2);
a = atan(x/y);
hfX = [r; a];
end
function [Xk, Pk, Kk] = ekf(Phikk_1, Qk, fXk_1, Pk_1, Hk, Rk, Zk_hfX) % ekf 滤波函数
Pkk_1 = Phikk_1*Pk_1*Phikk_1' + Qk;
Pxz = Pkk_1*Hk'; Pzz = Hk*Pxz + Rk; Kk = Pxz*Pzz^-1;
Xk = fXk_1 + Kk*Zk_hfX;
Pk = Pkk_1 - Kk*Pzz*Kk'