aigamozu
get GPS
Dependencies: AigamozuControlPackets XBee agz_common mbed
Diff: Kalmanprog.cpp
- Revision:
- 0:25715989dbc2
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalmanprog.cpp Wed Apr 08 13:16:59 2015 +0000
@@ -0,0 +1,222 @@
+/*
+#include "mbed.h"
+#include "XBee.h"
+
+#include "agz_common.h"
+#include "AigamozuControlPackets.h"
+
+
+#define MY_BASE_NUMBER 0
+
+XBee xbee(p13,p14);
+Serial pc(USBTX, USBRX); // tx, rx
+
+Ticker timer;
+
+//robot data
+AGZ_ROBOT robot[AGZ_NUM_ROBOTS];
+
+
+ double sigmaGPS[2][2] = {{250,0},{0,250}};
+ double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}};
+ double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}};
+ double y[2],x[2][2];
+
+
+//function
+//transform packetdata to robot data
+void agz_transform_papcket(uint8_t* packet_data,int id){
+ //set latitude, longitude, state
+ robot[id].set_state(packet_data[9]);
+ robot[id].set_LatitudeH(&packet_data[13]);
+ robot[id].set_LatitudeL(&packet_data[17]);
+ robot[id].set_LongitudeH(&packet_data[21]);
+ robot[id].set_LongitudeL(&packet_data[25]);
+}
+
+/*---------registere robot--------------*/
+/*
+void agz_register_robot(uint8_*t packet_data){
+ uint8_t robot_id = packet_data[5];
+ robot[robot_id].set_state(0);
+ send_register_ack();
+}
+*/
+
+/*---------timer set----*/
+/*
+void agz_send_request(){
+ static uint8_t next_send_address = 0,i;
+ AigamozuControlPackets agz_packet;
+
+ /*------search next moving robot--------*/
+ /*
+ for(i = next_send_address;i < AGZ_NUM_ROBOTS;i++){
+ if(robot[i].get_state() != 0xFF){
+ next_send_address = i;
+ break;
+ }
+ }
+ if(i == AGZ_NUM_ROBOTS){
+ for(i = 0;i < AGZ_NUM_ROBOTS;i++){
+ if(robot[i].get_state() != 0xFF){
+ next_send_address = i;
+ break;
+ }
+ }
+ }
+
+ agz_packet.createRequestCommand(MY_BASE_NUMBER,next_send_address);
+
+ ZBTxRequest agz_request_packet = ZBTxRequest(Agz_XBee_Remote_Address[next_send_address], agz_packet.packetData, REQUEST_COMMNAD_LENGTH );
+
+ xbee.send(agz_request_packet);
+
+ next_send_address++;
+}
+
+void get_K(){
+ double ad_bc = (sigma[0][0][0]+sigmaGPS[0][0])*(sigma[0][1][1]+sigmaGPS[1][1])-(sigma[0][1][0]+sigmaGPS[1][0])*(sigma[0][0][1]+sigmaGPS[0][1]);
+ K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(sigma[1][1][1]+sigmaGPS[1][1]);
+ K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(sigma[1][0][0]+sigmaGPS[0][0]);
+}
+
+void get_x(){
+ x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[1][0]);
+ x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[1][1]);
+}
+
+void get_sigma(){
+ double temp[2][2];
+ for(int i=0;i<2;i++) {
+ for(int j=0;j<2;j++) {
+ for(int k=0;k<2;k++) {
+ temp[i][j]+=K[1][i][k]*sigma[0][k][j];
+ }
+ }
+ }
+ for(int i = 0;i < 2;i++){
+ for(int j = 0;j < 2;j++){
+ sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
+ }
+ }
+}
+
+void Kalman(double Latitude,double Longitude){
+ y[0] = Latitude;
+ y[1] = Longitude;
+ //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
+ get_K();
+ //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
+ get_x();
+ //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
+ get_sigma();
+}
+
+
+/*---------init---------*/
+/*
+void agz_base_init(){
+ xbee.begin(57600);
+ pc.baud(57600);
+ timer.attach(&agz_send_request,1.0);
+
+}
+
+void Kalman_init(){
+}
+
+
+int main_Kalman(void){
+ int sender,reciever;
+
+
+ ZBRxResponse xbeeresponse;
+
+ agz_base_init();
+
+ wait(1);
+
+ printf("start\n");
+
+
+ //robot[0].set_state(0);
+ //robot[1].set_state(0);
+ //robot[2].set_state(0);
+ robot[3].set_state(0);
+ //robot[4].set_state(0);
+ //robot[5].set_state(0);
+
+ int count=0;
+
+
+ while(1){
+ xbee.readPacket();
+
+
+
+ if(xbee.getResponse().isAvailable() ){
+ xbee.getResponse(xbeeresponse);
+
+ //get data packet
+ if(xbee.getResponse().getApiId() == ZB_RX_RESPONSE){
+ uint8_t *packet_data = xbeeresponse.getData();
+ uint8_t packet_kinds = packet_data[3];
+
+ //get sensor data
+ if(packet_kinds == 'R'){
+ sender = packet_data[5]-1;// - 'A';
+ reciever = packet_data[7]-1;// - 'a';
+ agz_transform_papcket(packet_data,sender);
+
+ if(count==0){
+ count++;
+ x[1][0]=robot[sender].get_LatitudeL();
+ x[1][1]=robot[sender].get_LongitudeL();
+ }else if(count == 100){
+ printf("/////////////////////////Count = 100///////////////////////\n");
+ printf("state=%d,sender=%d,reciever=%d,latH=%ld.latL=%lf,longH=%ld.longL=%lf\n",robot[sender].get_state(),sender,reciever,
+ robot[sender].get_LatitudeH(),x[1][0],
+ robot[sender].get_LongitudeH(),x[1][1]);
+ for(int i = 0;i < 2;i++){
+ for(int j = 0;j < 2;j++){
+ printf("%lf ",sigma[0][i][j]);
+ }
+ printf("\n");
+ }
+ count = 0;
+ }else{
+ count++;
+ Kalman(robot[sender].get_LatitudeL(),robot[sender].get_LongitudeL());
+
+
+ printf("%d,%d,%d,%ld.%ld,%ld.%ld\n",robot[sender].get_state(),sender,reciever,
+ robot[sender].get_LatitudeH(),robot[sender].get_LatitudeL(),
+ robot[sender].get_LongitudeH(),robot[sender].get_LongitudeL());
+
+ printf("state=%d,sender=%d,reciever=%d,latH=%ld.latL=%lf,longH=%ld.longL=%lf\n",robot[sender].get_state(),sender,reciever,
+ robot[sender].get_LatitudeH(),x[1][0],
+ robot[sender].get_LongitudeH(),x[1][1]);
+ //kousinn
+ for(int i = 0;i < 2;i++){
+ for(int j = 0;j < 2;j++){
+ K[0][i][j]=K[1][i][j];
+ x[0][i]=x[1][i];
+ sigma[0][i][j]=sigma[1][i][j];
+ }
+ }
+ }
+ }
+ //get register packet
+ if(packet_kinds == 'E'){
+ //agz_register_robot(packet_data);
+ }
+ }else{
+ //get not rx response
+ }
+ }else{
+
+ }
+ }
+}
+*/
\ No newline at end of file