#include "kalman_rank2.h"
#include "string.h"
#include <stdio.h>
/*-------------------------------矩阵运算-------------------------------*/
/**
* @func 矩阵转置
* @brief A=A'
* @param m 行
* n 列
*/
void matrix_trans(double *A, double *B, unsigned char m, unsigned char n)
{
for (int i = 0; i < m; i++)
{
for (int j = 0; j < n; j++)
{
if (j >= i)
{
B[i * n + j] = A[j * n + i];
B[j * n + i] = A[i * n + j];
}
}
}
}
/**
* @func 矩阵加法
* @brief A+B=C
* @param m 行
* n 列
*/
void matrix_plus(double *A, double *B, double *C, unsigned char m, unsigned char n)
{
for (int i = 0; i < m; i++)
{
for (int j = 0; j < n; j++)
{
C[i * n + j] = A[i * n + j] + B[i * n + j];
}
}
}
/**
* @func 矩阵减法
* @brief A-B=C
* @param m 行
* n 列
*/
void matrix_minus(double *A, double *B, double *C, unsigned char m, unsigned char n)
{
for (int i = 0; i < m; i++)
{
for (int j = 0; j < n; j++)
{
C[i * n + j] = A[i * n + j] - B[i * n + j];
}
}
}
/**
* @func 矩阵乘法
* @brief A*B=C
* @param m 矩阵A的行
* p 矩阵A的列(也是矩阵B的行)
* n 矩阵B的列
*/
void matrix_multiply(const double *A, const double *B, double *C, unsigned char m, unsigned char p, unsigned char n)
{
for (int i = 0; i < m; i++)
{
for (int j = 0; j < n; j++)
{
C[i * n + j] = 0;
for (int k = 0; k < p; k++)
{
C[i * n + j] += A[i * p + k] * B[k * n + j];
}
}
}
}
/**
* @func 二阶矩阵求逆
* @brief B = A^-1
* @ret 0 可逆
* -1 不可逆
*/
int mareix_inv_rank2(double *A, double *B)
{
double div = A[0] * A[3] - A[1] * A[2];
if (div == 0)
return -1;
B[0] = A[3] / div;
B[1] = -A[1] / div;
B[2] = -A[2] / div;
B[3] = A[0] / div;
return 0;
}
/**
* @func 三阶矩阵求逆
* @brief B = A^-1
* @ret 0 可逆
* -1 不可逆
*/
int mareix_inv_rank3(double *A, double *B)
{
double div = A[0] * A[4] * A[8] + A[1] * A[5] * A[6] + A[2] * A[3] * A[7] - A[0] * A[5] * A[7] - A[1] * A[3] * A[8] - A[2] * A[4] * A[6];
if (div == 0)
return -1;
B[0] = (A[4] * A[8] - A[5] * A[7]) / div;
B[1] = -(A[1] * A[8] - A[2] * A[7]) / div;
B[2] = (A[1] * A[5] - A[2] * A[4]) / div;
B[3] = -(A[3] * A[8] - A[5] * A[6]) / div;
B[4] = (A[0] * A[8] - A[2] * A[6]) / div;
B[5] = -(A[0] * A[5] - A[2] * A[3]) / div;
B[6] = (A[3] * A[7] - A[4] * A[6]) / div;
B[7] = -(A[0] * A[7] - A[1] * A[6]) / div;
B[8] = (A[0] * A[4] - A[1] * A[3]) / div;
return 0;
}
/*-------------------------------卡尔曼运算-------------------------------*/
/**
* @func 卡尔曼滤波器初始化
* @ret kf 卡尔曼滤波器结构体
*/
void Kalman_Fil_Init(KalFil_t *kf)
{
memset(kf, 0, sizeof(KalFil_t));
kf->X[0][0] = 0;
kf->X[1][0] = 0;
kf->A[0][0] = 1;
kf->A[0][1] = Period;
kf->A[1][0] = 0;
kf->A[1][1] = 1;
kf->At[0][0] = 1;
kf->At[0][1] = 0;
kf->At[1][0] = Period;
kf->At[1][1] = 1;
kf->B[0][0] = 0.5 * Period * Period;
kf->B[1][0] = Period;
kf->H[0][0] = 1;
kf->H[0][1] = 0;
kf->Ht[0][0] = 1;
kf->Ht[1][0] = 0;
kf->Q[0][0] = 0.001;
kf->Q[0][1] = 0;
kf->Q[1][0] = 0;
kf->Q[1][1] = 0.001;
kf->R = 5;
kf->P[0][0] = 1;
kf->P[0][1] = 0;
kf->P[1][0] = 0;
kf->P[1][1] = 1;
kf->K[0][0] = 0;
kf->K[1][0] = 0;
kf->I[0][0] = 1;
kf->I[0][1] = 0;
kf->I[1][0] = 0;
kf->I[1][1] = 1;
}
/**
* @func 卡尔曼滤波器计算
* @ret kf 卡尔曼滤波器结构体
* ia 输入加速度(需要减去重力加速度)
* ih 输入位移
* @add 最终输出的位移、速度、存在结构体状态变量中
* 位移：X[0][0] 速度：X[1][0]
*/
void Kalman_Fil_Calc(KalFil_t *kf, double acc, double distance, int dis_update)
{
// 中间变量
double tempB[2][1] = {{0.5 * Period * Period}, {Period}};
double X1[2][1] = {0};
double X2[2][1] = {0};
double P1[2][2] = {0};
double P2[2][2] = {0};
double K1[2][1] = {0};
double K2[1][2] = {0};
double K3 = 0;
double X3 = 0;
double K4[2][1] = {0};
double P3[2][2] = {0};
kf->A[0][0] = 1;
kf->A[0][1] = Period; //dt
kf->A[1][0] = 0;
kf->A[1][1] = 1;
kf->At[0][0] = 1;
kf->At[0][1] = 0;
kf->At[1][0] = Period;
kf->At[1][1] = 1;
// 下一状态估计
matrix_multiply((double *)kf->A, (double *)kf->X, (double *)X1, 2, 2, 1); // X1 = A*x(k-1)
kf->B[0][0] = tempB[0][0] * acc; // B*u(k)
kf->B[1][0] = tempB[1][0] * acc;
matrix_plus((double *)X1, (double *)kf->B, (double *)X2, 2, 1); // X2 = A*x(k-1)+B*u(k)
// 计算协方差矩阵
// printf("kf->P:\n%f %f\n%f %f\n\n",kf->P[0][0],kf->P[0][1],kf->P[1][0],kf->P[1][1]);
matrix_multiply((double *)kf->A, (double *)kf->P, (double *)P1, 2, 2, 2); // P1 = A*kf->P
matrix_multiply((double *)P1, (double *)kf->At, (double *)P2, 2, 2, 2); // P2 = P1*kf->At
matrix_plus((double *)kf->Q, (double *)P2, (double *)P1, 2, 2); // P1 = P2 + Q
if (dis_update == 1)
{
printf("X2:\n%f\n%f\n\n",X2[0][0],X2[1][0]);
// 计算卡尔曼增益矩阵
matrix_multiply((double *)P1, (double *)kf->Ht, (double *)K1, 2, 2, 1); // K1 = P1*Ht
matrix_multiply((double *)kf->H, (double *)P1, (double *)K2, 1, 2, 2); // K2 = H*P1
K3 = K2[0][0] * kf->Ht[0][0] + K2[0][1] * kf->Ht[1][0]; // K3 = H*P1*Ht
double tempK = 1 / (K3 + kf->R); //1/[H*P1*Ht+R]
kf->K[0][0] = K1[0][0] * tempK; //K = P1*Ht*(1/[H*P1*Ht+R])
// kf->K[1][0] = K1[1][0] * tempK;
// 计算状态最优估计
X3 = kf->H[0][0] * X2[0][0] + kf->H[0][1] * X2[1][0]; // X3 = H * X2
double tempX = distance - X3; // Z- (H * X2)
K4[0][0] = kf->K[0][0] * tempX; //K*(Z- (H * X2))
K4[1][0] = kf->K[1][0] * tempX;
matrix_plus((double *)X2, (double *)K4, (double *)kf->X, 2, 2); //X = X2+ K*(Z- (H * X2))
// printf("K:\n%f\n%f\n\n",kf->K[0][0],kf->K[1][0]);
// printf("distance:%f,X3:%f,tempX:%f \n",distance,X3,tempX);
// printf("X:\n%f\n%f\n\n",kf->X[0][0],kf->X[1][0]);
}
else
{
// printf("X:\n%f\n%f\n\n",kf->X[0][0],kf->X[1][0]);
// printf("X2:\n%f\n%f\n\n",X2[0][0],X2[1][0]);
kf->X[0][0] = X2[0][0];
kf->X[1][0] = X2[1][0];
}
// 更新协方差矩阵
matrix_multiply((double *)kf->K, (double *)kf->H, (double *)P3, 2, 1, 2); // P3 = K*H
matrix_minus((double *)kf->I, (double *)P3, (double *)P2, 2, 2); // P2 = I - K*H
matrix_multiply((double *)P2, (double *)P1, (double *)kf->P, 2, 2, 2); // P = (I - K*H)*P1
// printf("========================================================================\n");
// printf("kf->I:\n%f %f\n%f %f\n\n",kf->I[0][0],kf->I[0][1],kf->I[1][0],kf->I[1][1]);
//printf("kf->P:\n%f %f\n%f %f\n\n",kf->P[0][0],kf->P[0][1],kf->P[1][0],kf->P[1][1]);
// printf("kf->B:\n%f\n%f\n\n",kf->B[0][0],kf->B[1][0]);
// printf("X2:\n%f\n%f\n\n",X2[0][0],X2[1][0]);
// printf("X:\n%f\n%f\n\n",kf->X[0][0],kf->X[1][0]);
//printf("K:\n%f\n%f\n\n",kf->K[0][0],kf->K[1][0]);
