国产精品亚洲香蕉第五区,一区二区久久,国产成人无码一区二区在线观看

/* Includes ------------------------------------------------------------------*/
#include "stm32f10x.h"
#include "kalman.h"
#include "matrix.h"
/*==============================================================================
1.預(yù)估計(jì)
X(k|k-1) = F(k,k-1)*X(k-1|k-1) //控制量為0
2.計(jì)算預(yù)估計(jì)協(xié)方差矩陣
P(k|k-1) = F(k,k-1)*P(k-1|k-1)*F(k,k-1)'+Q(k)
Q(k) = U(k)×U(k)'
3.計(jì)算卡爾曼增益矩陣
Kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R(k))
R(k) = N(k)×N(k)'
4.更新估計(jì)
X(k|k) = X(k|k-1)+Kg(k)*(Z(k)-H*X(k|k-1))
5.計(jì)算更新后估計(jì)協(xié)防差矩陣
P(k|k) =（I-Kg(k)*H）*P(k|k-1)
6. 更新最優(yōu)值
F(k,k-1): 狀態(tài)轉(zhuǎn)移矩陣
X(k|k-1): 根據(jù)k-1時(shí)刻的最優(yōu)值估計(jì)k時(shí)刻的值
X(k-1|k-1): k-1時(shí)刻的最優(yōu)值
P(k|k-1): X(k|k-1)對(duì)應(yīng)的covariance
P(k-1|k-1): X(k-1|k-1)對(duì)應(yīng)的covariance
Q(k): 系統(tǒng)過程的covariance
R(k): 測(cè)量過程的協(xié)方差
H(k): 觀測(cè)矩陣轉(zhuǎn)移矩陣
Z(k): k時(shí)刻的測(cè)量值
基本思路: 首先根據(jù)上一次(如果是第一次則根據(jù)預(yù)賦值計(jì)算)的數(shù)據(jù)計(jì)算出本次的估計(jì)值,
同理,根據(jù)上一次的數(shù)據(jù)計(jì)算出本次估計(jì)值的協(xié)方差; 接著,由本次估計(jì)值的協(xié)
方差計(jì)算出卡爾曼增益; 最后,根據(jù)估測(cè)值和測(cè)量值計(jì)算當(dāng)前最優(yōu)值及其協(xié)方差
==============================================================================*/
//================================================//
//== 最優(yōu)值方差結(jié)構(gòu)體 ==//
//================================================//
typedef struct _tCovariance
{
float PNowOpt[LENGTH];
float PPreOpt[LENGTH];
}tCovariance;
static tOptimal tOpt;
static tCovariance tCov;
//float Z[LENGTH] = {4000}; // 測(cè)量值(每次測(cè)量的數(shù)據(jù)需要存入該數(shù)組)
static float I[LENGTH] = {1}; // 單位矩陣
static float X[LENGTH] = {9.8}; // 當(dāng)前狀態(tài)的預(yù)測(cè)值
static float P[LENGTH] = {0}; // 當(dāng)前狀態(tài)的預(yù)測(cè)值的協(xié)方差
static float K[LENGTH] = {0}; // 卡爾曼增益
static float Temp3[LENGTH] = {0}; // 輔助變量
//============================================================================//
//== 卡爾曼濾波需要配置的變量 ==//
//============================================================================//
static float F[LENGTH] = {1}; // 狀態(tài)轉(zhuǎn)移矩陣即：堅(jiān)信當(dāng)前的狀態(tài)與上一次狀態(tài)的關(guān)系，如果堅(jiān)信是一樣的，則狀態(tài)轉(zhuǎn)移矩陣就為單位矩陣
static float Q[LENGTH] = {0.0001f};//0.0001f // 系統(tǒng)過程的協(xié)方差協(xié)方差的定義：真實(shí)值與期望值之差的平方的期望值
static float R[LENGTH] = {2}; // 測(cè)量過程的協(xié)方差協(xié)方差的定義：真實(shí)值與期望值之差的平方的期望值
//如果你需要濾波結(jié)果更依賴于觀測(cè)量，那就調(diào)小R，增大Q；反之，調(diào)大R，調(diào)小Q，這樣估計(jì)值就取決于系統(tǒng)。
//如果R大Q小，就是說，狀態(tài)估計(jì)值比測(cè)量值要可靠，這時(shí)，所得出的結(jié)果就是更接近估計(jì)值；
//如果R小Q大，這時(shí)，計(jì)算出來的結(jié)果就會(huì)更接近測(cè)量值。
static float H[LENGTH] = {1}; // 觀測(cè)矩陣轉(zhuǎn)移矩陣測(cè)量值與狀態(tài)預(yù)測(cè)值之間的單位換算關(guān)系，即把預(yù)測(cè)值單位換算成測(cè)量值單位
static float Temp1[LENGTH] = {1}; // 輔助變量, 同時(shí)保存tOpt.XPreOpt[]的初始化值
static float Temp2[LENGTH] = {10000}; // 輔助變量, 同時(shí)保存tCov.PPreOpt[]的初始化值
void KalMan_PramInit(void)
{
unsigned char i;
for (i=0; i<LENGTH; i++)
{
tOpt.XPreOpt[i] = Temp1[i]; //零值初始化
}
for (i=0; i<LENGTH; i++)
{
tCov.PPreOpt[i] = Temp2[i]; //零值初始化
}
}
//============================================================================//
//== 卡爾曼濾波 ==//
//============================================================================//
//==入口參數(shù): 當(dāng)前時(shí)刻的測(cè)量值 ==//
//==出口參數(shù): 當(dāng)前時(shí)刻的最優(yōu)值 ==//
//==返回值: 當(dāng)前時(shí)刻的最優(yōu)值 ==//
//============================================================================//
float KalMan_Update(float *Z)
{
u8 i;
MatrixMul(F, tOpt.XPreOpt, X, ORDER, ORDER, ORDER); // 基于系統(tǒng)的上一狀態(tài)而預(yù)測(cè)現(xiàn)在狀態(tài); X(k|k-1) = F(k,k-1)*X(k-1|k-1)
MatrixCal(F, tCov.PPreOpt, Temp1, ORDER);
MatrixAdd(Temp1, Q, P, ORDER, ORDER); // 預(yù)測(cè)數(shù)據(jù)的協(xié)方差矩陣; P(k|k-1) = F(k,k-1)*P(k-1|k-1)*F(k,k-1)'+Q
MatrixCal(H, P, Temp1, ORDER);
MatrixAdd(Temp1, R, Temp1, ORDER, ORDER);
Gauss_Jordan(Temp1, ORDER);
MatrixTrans(H, Temp2, ORDER, ORDER);
MatrixMul(P, Temp2, Temp3, ORDER, ORDER, ORDER);
MatrixMul(Temp1, Temp3, K, ORDER, ORDER, ORDER); // 計(jì)算卡爾曼增益; Kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)
MatrixMul(H, X, Temp1, ORDER, ORDER, ORDER);
MatrixMinus(Z, Temp1, Temp1, ORDER, ORDER);
MatrixMul(K, Temp1, Temp2, ORDER, ORDER, ORDER);
MatrixAdd(X, Temp2, tOpt.XNowOpt, ORDER, ORDER); // 根據(jù)估測(cè)值和測(cè)量值計(jì)算當(dāng)前最優(yōu)值; X(k|k) = X(k|k-1)+Kg(k)*(Z(k)-H*X(k|k-1))
MatrixMul(K, H, Temp1, ORDER, ORDER, ORDER);
MatrixMinus(I, Temp1, Temp1, ORDER, ORDER);
MatrixMul(Temp1, P, tCov.PNowOpt, ORDER, ORDER, ORDER); // 計(jì)算更新后估計(jì)協(xié)防差矩陣; P(k|k) =（I-Kg(k)*H）*P(k|k-1)
for (i=0; i<LENGTH; i++)
{
tOpt.XPreOpt[i] = tOpt.XNowOpt[i];
tCov.PPreOpt[i] = tCov.PNowOpt[i];
}
return tOpt.XNowOpt[0];
}

復(fù)制代碼