目录
本文主要是提供一维和二维卡尔曼滤波代码，故没有详细的原理解释，详细的解释可去B站查看
简介
卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。
基本概念
- 状态空间模型：卡尔曼滤波基于状态空间模型来描述系统。系统可以用状态方程和观测方程来表示。状态方程描述了系统状态如何随时间演变，观测方程描述了如何通过观测数据来获取关于系统状态的信息。
- 最优估计：目标是根据所有可用的观测数据，找到系统状态的最优估计，使得估计值在某种统计意义下最接近真实值，通常使用均方误差最小作为最优准则。
预测
-
- 状态预测：根据系统的动态模型，利用上一时刻的状态估计值来预测当前时刻的状态。假设系统的状态方程为xk=A xk - 1+B uk+wk，其中xk是k时刻的状态向量，A是状态转移矩阵，uk是k时刻的控制输入，B是控制输入矩阵，wk是过程噪声，且wk服从均值为0、协方差为Qk的高斯分布。则k时刻的状态预测值hatxk^-为hatxk^-=A\hatxk - 1+B uk。
- 协方差预测：状态估计的不确定性用协方差矩阵来表示。预测协方差矩阵\(P_{k}^{-}\)的更新方程为Pk^-=A Pk - 1A^T+Q_k，它反映了由于过程噪声和状态转移的不确定性导致的状态估计误差的增长。
更新
- 计算卡尔曼增益：卡尔曼增益Kk用于权衡预测值和观测值对最终估计值的贡献。它的计算基于预测协方差矩阵Pk^-、观测矩阵H和观测噪声协方差矩阵Rk，公式为Kk=Pk^-H^T(H Pk^-H^T+Rk)^-1。
- 状态更新：当获得新的观测值zk时，利用观测方程zk=H xk+vk，其中vk是观测噪声，且vk服从均值为0、协方差为Rk的高斯分布，来更新状态估计值。更新后的状态估计值hatxk为hatxk=hatxk^-+Kk(zk-Hhatxk^-)，其中z_k-H\hatxk^-是观测残差，即观测值与预测值之间的差异。
- 协方差更新：更新后的协方差矩阵Pk为Pk=(I - KkH)Pk^-，其中I是单位矩阵。这一步调整了协方差矩阵，以反映更新后的状态估计的不确定性。
一阶和二阶卡尔曼滤波代码
///.c///
#include "Kalman.h"
/*
* @brief
* 初始化结构体@kalman1_state的字段。
* 在此初始化函数中我设置了一些默认值：
* A = 1；
* H = 1；
* 并且@q,@r在先前的测试后被赋值。
*
* 注意：请根据你的应用程序更改 A、H、q、r。
*
* @inputs
* state - 卡尔曼滤波器结构
* init_x - 初始 x 状态值
* init_p - 初始估计误差协方差
* @outputs
* @retval
*/
void kalman1_init(kalman1_state *state, float init_x, float init_p)
{
state->x = init_x;
state->p = init_p;
state->A = 1;
state->H = 1;
state->q = 2e2;//10e-6; /* predict noise convariance */
state->r = 5e2;//10e-5; /* measure error convariance */
}
/**
* @brief
* 一维卡尔曼滤波器
* @inputs
* state - 卡尔曼滤波器结构
* z_measure - 测量值
* @outputs
* @retval
* 估计结果
*/
float kalman1_filter(kalman1_state *state, float z_measure)
{
/* Predict */
state->x = state->A * state->x;
state->p = state->A * state->A * state->p + state->q; /* p(n|n-1)=A^2*p(n-1|n-1)+q */
/* Measurement */
state->gain = state->p * state->H / (state->p * state->H * state->H + state->r);
state->x = state->x + state->gain * (z_measure - state->H * state->x);
state->p = (1 - state->gain * state->H) * state->p;
return state->x;
}
/*
* @brief
* 初始化结构 @kalman1_state 的字段。
* 在这个初始化函数中我设置了一些默认值：
* A = {{1, 0.1}, {0, 1}};
* H = {1,0};
* 并且 @q,@r 在先前的测试后被赋值。
*
* 注意：请根据你的应用程序更改 A、H、q、r。
*
* @inputs
* @outputs
* @retval
*/
void kalman2_init(kalman2_state *state, float *init_x, float (*init_p)[2])
{
state->x[0] = init_x[0];
state->x[1] = init_x[1];
state->p[0][0] = init_p[0][0];
state->p[0][1] = init_p[0][1];
state->p[1][0] = init_p[1][0];
state->p[1][1] = init_p[1][1];
//state->A = {{1, 0.1}, {0, 1}};
state->A[0][0] = 1;
state->A[0][1] = 0.1;
state->A[1][0] = 0;
state->A[1][1] = 1;
//state->H = {1,0};
state->H[0] = 1;
state->H[1] = 0;
//state->q = {{10e-6,0}, {0,10e-6}}; /* measure noise convariance */
state->q[0] = 10e-7;
state->q[1] = 10e-7;
state->r = 10e-7; /* estimated error convariance */
}
/*
@brief
* 二维卡尔曼滤波器
* @inputs
* state - 卡尔曼滤波器结构
* z_measure - 测量值
* @outputs
* state->x[0] - 更新后的状态值，例如角度、速度
* state->x[1] - 更新后的状态值，例如角度差、加速度
* state->p - 更新后的估计误差协方差矩阵
* @retval
* 返回值等于 state->x[0]，所以可能是角度或速度。
*/
float kalman2_filter(kalman2_state *state, float z_measure)
{
float temp0 = 0.0f;
float temp1 = 0.0f;
float temp = 0.0f;
/* Step1: Predict */
state->x[0] = state->A[0][0] * state->x[0] + state->A[0][1] * state->x[1];
state->x[1] = state->A[1][0] * state->x[0] + state->A[1][1] * state->x[1];
/* p(n|n-1)=A^2*p(n-1|n-1)+q */
state->p[0][0] = state->A[0][0] * state->p[0][0] + state->A[0][1] * state->p[1][0] + state->q[0];
state->p[0][1] = state->A[0][0] * state->p[0][1] + state->A[1][1] * state->p[1][1];
state->p[1][0] = state->A[1][0] * state->p[0][0] + state->A[0][1] * state->p[1][0];
state->p[1][1] = state->A[1][0] * state->p[0][1] + state->A[1][1] * state->p[1][1] + state->q[1];
/* Step2: Measurement */
/* gain = p * H^T * [r + H * p * H^T]^(-1), H^T means transpose. */
temp0 = state->p[0][0] * state->H[0] + state->p[0][1] * state->H[1];
temp1 = state->p[1][0] * state->H[0] + state->p[1][1] * state->H[1];
temp = state->r + state->H[0] * temp0 + state->H[1] * temp1;
state->gain[0] = temp0 / temp;
state->gain[1] = temp1 / temp;
/* x(n|n) = x(n|n-1) + gain(n) * [z_measure - H(n)*x(n|n-1)]*/
temp = state->H[0] * state->x[0] + state->H[1] * state->x[1];
state->x[0] = state->x[0] + state->gain[0] * (z_measure - temp);
state->x[1] = state->x[1] + state->gain[1] * (z_measure - temp);
/* Update @p: p(n|n) = [I - gain * H] * p(n|n-1) */
state->p[0][0] = (1 - state->gain[0] * state->H[0]) * state->p[0][0];
state->p[0][1] = (1 - state->gain[0] * state->H[1]) * state->p[0][1];
state->p[1][0] = (1 - state->gain[1] * state->H[0]) * state->p[1][0];
state->p[1][1] = (1 - state->gain[1] * state->H[1]) * state->p[1][1];
return state->x[0];
}
///.h///
#ifndef _KALMAN_H_
#define _KALMAN_H_
#include "stdlib.h"
#include "arm_math.h"
/*
* NOTES: n Dimension means the state is n dimension,
* measurement always 1 dimension
* 注意：n 维意味着状态是 n 维，测量始终是 1 维。
*/
/* 1 Dimension */
typedef struct {
float x; /* state */
float A; /* x(n)=A*x(n-1)+u(n),u(n)~N(0,q) */
float H; /* z(n)=H*x(n)+w(n),w(n)~N(0,r) */
float q; /* process(predict) noise convariance */
float r; /* measure noise convariance */
float p; /* estimated error convariance */
float gain;
} kalman1_state;
/* 2 Dimension */
typedef struct {
float x[2]; /* state: [0]-angle [1]-diffrence of angle, 2x1 */
float A[2][2]; /* X(n)=A*X(n-1)+U(n),U(n)~N(0,q), 2x2 */
float H[2]; /* Z(n)=H*X(n)+W(n),W(n)~N(0,r), 1x2 */
float q[2]; /* process(predict) noise convariance,2x1 [q0,0; 0,q1] */
float r; /* measure noise convariance */
float p[2][2]; /* estimated error convariance,2x2 [p0 p1; p2 p3] */
float gain[2]; /* 2x1 */
} kalman2_state;
extern void kalman1_init(kalman1_state *state, float init_x, float init_p);
extern float kalman1_filter(kalman1_state *state, float z_measure);
extern void kalman2_init(kalman2_state *state, float *init_x, float (*init_p)[2]);
extern float kalman2_filter(kalman2_state *state, float z_measure);
#endif
总结
卡尔曼滤波通过不断地进行预测和更新操作，利用系统的先验知识（状态方程和噪声统计特性）和实时观测数据，逐步地修正对系统状态的估计，使得估计结果能够尽可能准确地跟踪系统的真实状态，在存在噪声和不确定性的情况下，提供最优的状态估计。
扫一扫
4971
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
